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The overall goal of "Telerobotic Control of a Mobile Coordinated Robotic Servicer^ qqQ 
project is to develop advance control methods that would enhance the usage of robotic systems ^ 
for space applications. Towards this end, several algorithms have been developed from this 
project. One area of development was to extend the methodology of the Observer/Kalman Filter 
Identification (OKID) approach, developed at NASA Langley, to such design problems as 
frequency spectrum reconstruction, improved parameter estimation from frequency data and 
recursion structures to improve computational performance. This area addressed the 
identification issue of systems which can then be followed by regulation design as is typical in 
self-tuning adaptive control. The approach has applicability to many types of systems, including 
robotics, when the system structure or parameter set is unknown or has variations. 

The second area of control research focused on fuzzy control which is a non-parametric 
(non-model-based) knowledge-based approach. In this area, adaptive algorithms were developed 
using self-tuning scaling factor schemes in the fuzzifier, self-learning schemes in the control 
rulebase and optimization to extend the method to multi-input, multi-output systems. As a 
knowledge-based approach, the MIMO adaptive fuzzy controller uses a computationally efficient 
rulebase to determine control commands when the system model (the robot dynamics) is partially 


unknown or varies with time. 

The final phase of this effort was devoted to the design, fabrication and testing of a robot 
manipulator arm which is attached to a mobile robotic system, a rover, built at the Mars Mission 
Research Center. The rover is currently under teleoperation mode and will have capabilities for 
full autonomy. The manipulator arm along with the mobile robotic system will be used to test all 
of the control algorithms that have been developed though this effort as well as other programs at 
the Mars Mission Research Center. 

What follows is the MS thesis of Mr. Mike Brown. Mike spent a summer at NASA 
Langley working in the Spacecraft Dynamics Branch. His thesis develops the design and testing 
of the manipulator arm on the teleoperated mobile robotic system. 



ABSTRACT 


BROWN, JR., ROBERT MICHAEL. A Microcontroller-Based Three Degree-of- 
Freedom Manipulator Testbed. (Under the direction of Gordon K. F. Lee.) 

A wheeled exploratory vehicle is under construction at the Mars Mission 
Research Center at North Carolina State University. In order to serve as more 
than an inspection tool, this vehicle requires the ability to interact with its 
surroundings. A crane-type manipulator, as well as the necessary control 
hardware and software, has been developed for use as a sample gathering tool 
on this vehicle. The system is controlled by a network of four Motorola 
M68HC1 1 microcontrollers. Control hardware and software were developed in a 
modular fashion so that the system can be used to test future control algorithms 
and hardware. Actuators include three stepper motors and one solenoid. 
Sensors include three optical encoders and one cable tensiometer. 

The vehicle supervisor computer provides the manipulator system with the 
approximate coordinates of the target object. This system maps the workspace 
surrounding the given location by lowering the claw, along a set of evenly spaced 
vertical lines, until contact occurs. Based on this measured height information 
and prior knowledge of the target object size, the system determines if the object 
exists in the searched area. The system can find and retrieve a 1.25 in diameter 
by 1.25 in tall cylinder placed within the 47.5 in 2 search area in less than 12 
minutes. This manipulator hardware may be used for future control algorithm 
verification and serves as a prototype for other manipulator hardware. 
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Chapter One: INTRODUCTION 


Robotic vehicles are ideal for the exploration of hostile environments. 
These devices allow humans to investigate areas that would otherwise be 
difficult or impossible to reach. In order to serve as more than inspection tools, 
these robots must have the ability to interact with their surroundings. An 
undersea vehicle on a scientific mission must often collect sediment and water 
samples 1 . A Space Station assembly vehicle must be able to position and 
connect building materials. An emergency response robot could open doors and 
move debris while searching for injured victims in a burning building. A robotic 
vehicle in a hazardous material spill area could be used to locate and close a 
critical valve. 

A. Background Information 

Three basic types of joints, revolute, prismatic, and suspended cable, are 
typically used by manipulator systems. Revolute joints, like a human elbow, 
rotate about an axis. Prismatic joints, like an extension ladder, extend or retract 
along a linear path. Suspended cable systems, used in place of rigid structural 
members on crane systems, also extend or retract. The key difference is that 
the path followed by a payload suspended by a cable is a function of 
gravitational effects and environmental disturbances. 

A revolute joint system, such as the manipulator arm used on the NASA 
space shuttle, is very maneuverable. The variable direction of approach, made 



possible by the slender structural members and multiple revolute joints, allows 
the retrieval of unsymmetrical payloads. In the weightless environment of space, 
the joint actuators must position the end-effector and damp unwanted motion. 
However, in a similar system operating vertically in a gravity field, the joint 
actuators must also support the manipulator structure and payload. Lower 
payload capacities, relative to a crane with identical actuators, result. 

Prismatic joints are often used in systems where precision is more 
important than range of motion. Extremely fine control of an end effector 
trajectory is possible with rigid links and prismatic joints. This advantage is 
gained at the expense of mechanical complexity and additional weight. Loss of 
mobility also results since the distance that a joint can extend is limited by the 
length of the telescoping member. 

Suspended cable joints, found in crane systems, are capable of extreme 
ranges of motion. Cable, unlike rigid members, can be stored in great lengths on 
winch drums. Since the structure, not the actuators, of a crane carries bending 
loads, relatively high payload-to-system-weight ratios can be achieved. The key 
disadvantages of a crane system are the difficulties in controlling all six degrees- 
of-freedom of the end effector and in damping undesired motion. 

The National Institute of Standards and Technology (NIST) has developed 
a six degree-of-freedom crane called ROBOCRANE 2 . This system uses cables 
as structural links, winches as actuators, and cable travel encoders as sensors. 
A cable, a winch actuator, and an encoder are required for each controlled 
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degree-of-freedom to ensure a fully constrained system. This technology is 
applicable to various types of crane platforms, such as tower, boom, and 
overhead, as well as lower degree-of-freedom systems. 

Depending on system requirements and research objectives, algorithms 
used in crane control vary from classical to discrete 3 to fuzzy logic 4 schemes. 
The Motorola M68HC1 1 is a relatively inexpensive microcontroller allowing the 
use of both classical 5,6 and fuzzy control techniques 7 . 

B. Research Objectives and Problem Development 

An autonomous wheeled exploratory vehicle is currently under 
construction at the Mars Mission Research Center at North Carolina State 
University 8,9,10 . This vehicle, pictured in Figure 1-1, will be tasked with the 
exploration of unfamiliar terrain. In order to effectively carry out this mission, the 
vehicle must avoid dangers, such as boulders and crevasses, gather information, 
such as visual images and sensor data, and collect physical samples, such as 
rock and soil. The latter mission requirement makes a manipulator subsystem 
necessary. 

The purpose of the research presented in this thesis is to develop a 
manipulator that serves two purposes. The first goal is to provide the required 
environmental sample gathering tool for a Mars vehicle prototype. The second 
goal is to provide a platform for future robotic manipulator research activities. A 
crane-type manipulator system configuration was selected to insure adequate 
payload capacity. Motorola microcontrollers were selected so that the prototype 
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payload capacity. Motorola microcontrollers were selected so that the prototype 
system will be capable of implementing both traditional and modern control 
techniques. The structural hardware, electrical hardware, and control software 
have been designed and constructed in a modular fashion. Future researchers 
will be able to further optimize the system by modifying individual hardware and 
software components. 



Figure 1-1: NCSU Mars Rover 

Based on the existing vehicle design, the following criteria must be met by 
the vehicle and manipulator subsystem. 

1 ) The vehicle must 

• Provide 1 2 V and 48 V electrical power. 
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• Provide an ASCII string, via a serial link, containing manipulator 
platform pitch and roll as well as object location in cylindrical 
coordinates. The string format will be ±pp,±qq,±G96,±rr.r,±zz.z. 

• Disable the wheel motor subsystem during manipulator operation. 

• Confirm retrieval of desired object with vehicle sensor devices, 
such as vision or ultrasound. 

2) The manipulator system must 

• Fit in a space that measures 18 in long by 8 in wide by 18 in tall. 

• Weigh no more than 20 lb. 

• Find and retrieve a typical environmental sample, approximated by 
a 1 .25 in diameter by 1 .25 in tall cylinder, when provided with a 
target location inside the workspace and within 4 in of actual object 
location. 

• Be capable of lifting a payload weighing up to 1 lb. 

C. Thesis Organization 

This thesis is divided into seven chapters. Chapter 2, System Hardware 
Description, describes mechanical and electrical system components. In 
Chapter 3, Control Software Description, a discussion is presented on the 
software embedded in each of the four controllers. Chapter 4, Application of 
Control Software to Crane System, details the system tests and results. Finally, 
Chapter 5, Conclusions and Suggestions for Future Work, states conclusions 
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and offers ideas for system improvements. Appendices contains commented 
computer code for each type of controller and programming instructions. 
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Chapter Two: SYSTEM HARDWARE DESCRIPTION 

The manipulator system is composed of the master controller, three motor 
controllers, three motor drivers, three stepper motors, three optical encoders, a 
solenoid actuated claw, a tensiometer, and the crane structure. These 
components are pictured in Figure 2-1 and are discussed in the following 
sections of this chapter. 



Figure 2-1 : Manipulator System 


A. Crane Structure 

Four major components, illustrated in Figure 2-2, make up the structure of 
the manipulator. The tower is the vertical structure about which the boom pivots. 
The lower flange of the boom acts as a track for the trolley. The claw is 
suspended on a aramid fiber cable from the trolley. The tower, boom, and trolley 
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are constructed of readily available components to allow modification by future 
users. The main structural components are formed from prefabricated fiberglass 
I-beam, channel, and angle stock. The boom and cable drum are driven directly 
by their respective motors. The trolley position is controlled via a chain drive with 
the third motor. 



Figure 2-2: Crane Structure 


B. Coordinate System and Workspace 

The three coordinates used to describe the position of the closed 
manipulator claw tips are illustrated in Figure 2-3. The radius (r) and angle (0) 
are standard polar coordinates when the system is viewed from above. The 
radius is measured from the rotational axis to the trolley center. The angle is 
measured counter-clockwise from home position. Note that the angle illustrated 
in Figure 2-3 is in the negative direction. The height (z) is the distance from the 
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baseplane to the claw tips, where a positive value of z is used for points above 
the plane. 



C. Actuators 

Stepper Motors and Drivers 

The three positioning actuators in this device are Pacific Scientific 
Powermax P21NRXA-LDF-M1-00 stepper motors. Each is driven by a Pacific 
Scientific Sigma Model 5210 motor driver. Each motor requires 2.5 A at 12 V 
and provides a holding torque of 1 14 oz-in. In addition to power, the motor driver 
requires two logic inputs. The level of the direction input determines the direction 
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of rotation. A square wave applied to the second input results in a motor step for 
every wave period. 

The motor responsible for lifting the claw and captured object was 
selected to maximize the payload capacity of the system. Identical motors were 
selected to control boom and trolley location, to standardize hardware, and to 
increase modularity. 

Solenoid Actuated Claw 

The claw, illustrated in Figure 2-4, is the same type used in arcade 
games. A more forgiving control system and minor claw modification result in a 
much better success rate. The claw has three fingers, located 120° apart, that 
are activated by a 48V solenoid. Original tests, involving a range of object types, 
demonstrated that, while the claw was very effective at “scooping” up a large 
object, such as a four inch diameter sphere, it was not capable of holding most 
smaller objects in its fingertips. To include small objects in the target range, 
these fingers were modified by the addition of claw tips. These tips, acting as 
fingernails, are 0.063 inch diameter rods protruding one-quarter inch from the 
fingertips. They result in a great enhancement in gripping capability. 


10 




Figure 2-4: End Effector With Sample Object 


D. Sensors 
Optical Encoders 

A U. S. Digital Model E2-512-250-IE optical encoder is mounted on the 
shaft of each stepper motor. The resolution of each encoder is 512 counts per 
revolution. An index pulse, once per revolution, allows the motor controllers to 
find home position from the power-on position. Sensor output consists of two 
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square wave signals that, except for phase, are identical. The lead-lag 
relationship of these two signals reflects the direction of motion of the encoder. 
In general, these signals can be decoded by the motor controller. However, due 
to high frequency “ringing” of the stepper motor after a single step command, a 
separate chip was used. This chip, an LSI Computer Systems LS7166 24 bit 
multimode counter, can accurately decode the encoder signals even with the 
high frequency changes in direction of encoder rotation associated with ringing. 
The internal 24 bit counter containing the motor position can be read by the 
motor controller via an 8 bit data bus. The use of this chip relieves the 
microcontroller of the burden of constantly monitoring the encoder output. 

Cable Tensiometer 

A slight loss of cable tension, such as occurs when the claw makes 
contact with some surface or object, causes an interrupt service routine on the 
master controller to be activated. A discussion of this software routine can be 
found in Chapter 3. Figure 2-5 illustrates the mechanical components of the 
sensor. The key electrical component that enables this interrupt is a conditioned 
single-pole double-throw (SPDT) switch. This switch is mounted on a lever 
whose position is controlled by the cable tension. The switch and conditioning 
circuit 11 , detailed in Appendix A, control the state of pin PA3 on the master 
controller. This pin has input capture capabilities that are used to trigger the 
interrupt service routine. 
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Figure 2-5: Tensiometer Hardware 


E. Microcontrollers 

Controller hardware consists of one master controller and three motor 
controllers. These components are pictured in Figure 2-6 and are discussed in 
the following sections. 
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Figure 2-6: Controller Hardware 


Motorola 68HC1 1 E9 General Description 

The Motorola 68HC11E9 is a one of a family of devices called 
microcontrollers or MCUs. An MCU combines discrete communication circuitry, 
a processor, a data bus, and memory into a small, low power, single chip 
computer. The 68HC11 MCU has 40 input/output pins that allow serial and 
parallel communication and analog-to-digital conversion. The 16 bit memory 
address of the 68HC1 1 allows the use of 64K bytes of memory. Internal memory 
in the 68HC11E9 consists of 512 bytes of random access memory (RAM), 512 
bytes of electronically erasable programmable read only memory (EEPROM), 
12K bytes of erasable programmable read only memory (EPROM), a 64 byte 
register block, and a 64 byte bootstrap interrupt vector block. The remainder of 
the 64K space can be accessed using an external memory chip. Detailed 
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hardware information can be found in M68HC1 1 Reference Manual 12 , M68HC1 1 
E Series Technical Data 13 , and M68HC11 E Series Programming Reference 
Guide 14 . 

While hardware defines the limits of a microcontroller’s capability, the 
software is the tool necessary to realize these limits. Program size, execution 
speed, mathematical capability, and design are all important considerations 
when measuring the effectiveness of any computer code. 

Code for the 68HC11 can be written using a number of high level 
languages and assemblers. While languages such as Lisp and C provide data 
structures and mathematical functions that allow the intuitive coding of complex 
behavior, Motorola Assembly Language was used to generate all code used in 
this project. The primary reason for this selection was to simplify code 
troubleshooting. Debugging code in the PCbugl 1 or BUFFALO environment is 
straightforward using the disassembler. When using this feature, code is viewed 
as assembly code regardless of the original language. The usable result of 
debugged and assembled code is an ASCII file in S-record format. This 
machine language version of the original program can be read, edited, and 
transported to the MCU. 

In order to program and debug the MCU, a Universal Evaluation Board 
(EVBU) is used . This board, with an MCU inserted, has many of the same 
functions as the final version of the motor controller. It provides the MCU with 
regulated power, a RS-232 serial interface, an oscillator, and access to all MCU 
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pin logic levels. Since the MCU is a self-contained computer, some software 
must be present and running before any meaningful communication with any 
other system can occur. Two software applications, provided by Motorola with 
the EVBU, are BUFFALO and PCbugll. While allowing the user to perform 
nearly the same tasks, these two applications work in very different ways. 

BUFFALO is a complex piece of code that must be previously loaded into 
MCU memory. When the EVBU is reset, BUFFALO begins execution. This 
program allows the user to use a VT100 terminal emulation program and serial 
link to connect to the MCU. Once the connection is established, code can be 
loaded into RAM or EEPROM, executed, and debugged. The two most 
significant limitations of BUFFALO are that it must already be loaded into MCU 
memory and that it cannot modify EPROM. BUFFALO is well documented and 
discussed in the User’s Manual provided with the EVBU 15 . 

PCbugll is a DOS-based application capable of connecting to an 
unprogrammed MCU. During initialization, a small program, called a talker, is 
loaded in to MCU memory. This small but powerful piece of code, allows the 
user to read and program any available MCU memory location in RAM, 
EEPROM, or EPROM. Since only a small portion of memory is used temporarily 
for the talker, a much larger block of code can be transferred to MCU memory. 
The use of PCbugl 1 is documented in the PCbugl 1 User’s Manual . 

One important feature of the MCU is the receiver wake-up operation. 
When multiple controllers are used, every controller receives any message sent 
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by a controller on the network. A system must be devised to allow a receiving 
MCU to determine if it is being addressed. The address-mark wake-up feature, 
available on the 68HC1 1 MCU, solves this dilemma. Each MCU is placed in a 
dormant state by enabling the RWU bit in the SCCR2 register. In order to select 
any controller, a byte of information must be sent in which the most significant bit 
is set. The remaining seven bits are used as a coded address. This byte of 
information will wake up each controller. The software running on each MCU is 
responsible for determining if the encoded address matches its own. If no match 
exists, the software is responsible for placing the MCU back in the dormant 
mode. 

Master Controller 

The master controller includes a 5 V voltage regulator, a M68HC11 
microcontroller, a crystal oscillator circuit, and a reset circuit. In addition, the 
master controller, detailed in Appendix A, also contains an external 32K RAM 
chip, a claw solenoid activation circuit, the conditioning circuit for the tensiometer 
switch, and serial communications hardware for five serial ports. The external 
RAM allows for increased program size and faster reprogramming time than 
using internal EPROM. Using this external memory requires that the MCU be 
used in expanded mode. As a result, ports B and C are no longer usable as 
external I/O pins. The claw activation circuit consists of a transistor driven relay 
switch. The conditioning circuit for the tensiometer is discussed in the 
tensiometer section. The communication hardware consists of three RS-232 
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serial port drivers and one asynchronous communications interface adapter 
(ACIA). Two of the RS-232 drivers each control two serial ports. These four 
ports, connected to the TxD and RxD MCU pins, are used to communicate with 
the motor controllers. The last driver is used, along with the ACIA, to allow serial 
communication with the supervisor computer via the Port C data bus. 

Motor Controller 

Like the master controller, the motor controller, detailed in Appendix B, 
contains a 5 V voltage regulator, a M68HC11 microcontroller, a crystal oscillator 
circuit, and a reset circuit. In addition, it contains optical encoder decoding circuit 
and one RS-232 serial driver. The decoding circuit is discussed in the section on 
optical encoders. The single serial driver is used to allow communication with 
the master controller via the RxD and Txd MCU pins. 
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Chapter Three: CONTROL SOFTWARE DESCRIPTION 

Each of the four microcontrollers contains embedded control software. 
These programs were coded in assembly language for the Motorola M68HC1 1 
series microcontroller. Each program is written as a text file, assembled, and 
downloaded to the microcontroller RAM, EPROM, or EEPROM using PCBUG11 
or BUFFALO. A listing of the code used in the master controller can be found in 
Appendix C. A listing of the code used in a typical motor controller can be found 
in Appendix D. Appendix E contains Motorola application notes outlining the 
steps necessary to write, assemble, store, and run a piece of sample code. 

The master controller contains the code necessary to service the 
supervisor computer, the claw actuator and sensor, and each of the three motor 
controllers. The motor controller code, identical in each case except for 
constants defining position limits, controller address, and motor speed, is 
responsible for driving and sensing motor position and communicating with the 
master controller. 

Since the exploratory vehicle system is currently under construction, an 
IBM-compatible 486DX-33 personal computer running PROCOMM terminal 
emulation software is used in place of the supervisor computer. Any computer 
with a 9600 baud serial connection and software to access that port can be 
substituted for the supervisor computer. 
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Discussion of the two programs is divided into four sections. Each section 
focuses on the algorithm used to control the interaction between two hardware 
component systems. 

A. Supervisor / Master Controller Loop 

The link between the supervisor and the master controller has two 
functions. First, the supervisor must provide the master controller with an ASCII 
string containing platform orientation and target coordinates. Second, the 
master controller provides claw trajectory information that can be used for 
system monitoring. The supervisor must examine the sample object and 
resubmit the command if the correct object was not retrieved. 

All data transfer is accomplished via a 9600 baud serial connection. Data 
strings are in ASCII format to allow easier debugging and system monitoring. 
These strings are converted into hexadecimal values upon receipt by the master. 

Each time the master receives information from a motor controller or 
sensor, a character string is sent to the supervisor. This string contains position 
information for each motor as well as the current state of the claw actuator. 
Motor positions are written as absolute angles, in degrees, in hexadecimal form. 

B. Master Controller / Motor Controller Loop 

The master controller, upon receiving the approximate target location, 
calculates the desired motor positions. These positions are functions of the 
desired position (0,r,z) d and platform orientation (p,q). The master algorithm 
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approximates the motor positions by assuming that the platform is level during 
manipulator operation. This assumption decouples the effects of motor positions 
on claw position. Each motor is assumed to control one degree-of-freedom and 
have no effect on the other two degrees-of-freedom. 

Since the claw is suspended on a cable, it will always move along a 
vertical line. When pitch and roll are both zero, this vertical line is parallel with 
the z axis. In this situation, the distance that the claw must be extended or 
retracted is a function only of the desired z coordinate. Similarly, the trolley 
motor only affects r and the boom motor only affects 9. When some platform 
pitch or roll exists, there will be an error in claw position whose magnitude varies 
with motor positions and platform orientation. Since the object of the maneuver 
is not to reach some given position, but rather to find some object within the 
search area, the claw position error only becomes a problem if it is sufficient to 
position the search area away from the target object. The amount of error that is 
acceptable in the system is a function of the search pattern area and grid 
resolution. 

Given that the platform orientation is neglected, the system is completely 
decoupled. As a result, each motor controls a single degree-of-freedom. The 
desired motor positions are calculated based on measured values of trolley and 
claw travel in degrees per inch, measured motor angles when the claw is in the 
home position, and the desired cylindrical coordinates. Values for these 
constants can be found the software listings included in Section 0. 
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The master controller issues all commands to the motor controllers and 
claw in a serial fashion. After sending each motor command, the master waits 
for the motor controller to achieve and feedback its desired position. Two 
routines, docmds and docmds2, are used to issue a string of commands. The 
first, docmds, performs them in the order of boom motion, trolley motion, claw 
motion, and claw activation. The second, docmds2, commands the motors in 
the reverse order and but still activates the claw last. The first routine is used to 
approach an object. Since this object could be in a depression, the claw is kept 
at as high as possible until directly over the target site. The second routine, used 
after the object is captured, lifts the claw completely before moving the boom or 
trolley. 

A portion of the main routine of the master controller software, called 

whenever a valid target position input string is received, is listed below. 

jsr findit 
jsr putaway 
jsr gohome 

The first routine, findit, is responsible for searching for and grasping the object. 
The search algorithm is addressed in Chapter 4. The second routine, putaway, 
directs the claw to move to a receptacle and release the object. The last routine, 
gohome, sends the claw back to its home position. 

C. Motor Controller / Motor Loop 

The motor controller receives a hexadecimal number, in ASCII characters, 
representing the desired absolute angle of the motor. The actual motor position 
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is read from the decoder chip and converted into degrees. These two values are 
compared and a desired rotation, in degrees, is calculated. This number is 
converted into motor steps. The motor controller then drives the direction pin on 
the motor driver high for forward motion or low for reverse motion. A square 
wave is then applied to the driver input pin. The number of pulses in this wave 
corresponds to the number of desired steps. The frequency of the wave 
determines motor speed. The motor controller again reads the decoder chip and 
the process is repeated as necessary. An error of 1° is allowed between desired 
and final actual motor position. This allowance is necessary due to the encoder 
resolution and integer division necessary to convert the encoder value to 
degrees. When the final position is attained, the motor controller echoes its 
current absolute position to the master controller. 

D. Master Controller / Claw Loop 

The master controller, in addition to performing high level motor position 
control and communication with the supervisor, is responsible for claw activation 
and obstacle contact detection. The claw is commanded via a relay on the 
master controller circuit board. Contact between the claw and some obstacle is 
monitored via a boom mounted switch and conditioning circuitry mounted on the 
master controller circuit board. 

The master controller enables or disables the claw by varying the state of 
one of the microcontroller output pins. The pin indirectly drives the claw solenoid 
using a transistor and a relay. 
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A sudden and sustained loss of tension occurs as a result of contact 
between the claw and some surface. The loss of cable tension causes the 
activation of an interrupt service routine on the master controller to command the 
motor controller to stop and slightly raise the claw. This motion allows the claw 
to better grip the target object. 

This feature is vital to the success of the searching algorithm. The daw is 
closed to minimize the projected area on the work surface and to limit contact to 
one point instead of three points. The claw is then lowered at predetermined 
points until contact is made. The absolute heights of these points are stored 
until all points are searched. After all nine heights are measured, they are 
converted into quarters of an inch above the lowest of the nine points. The 
predetermined object height is 1.25 in. Software selects the first of the nine 
points that happens to be higher than 1 in. If no object tall enough is found then 
the search pattern may be repeated at another location. In general, a complete 
search includes the mapping of 47.5 in 2 . The search pattern and mapping 
technique are discussed in more detail in Chapter 4. 
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Chapter Four: APPLICATION OF CONTROL SOFTWARE TO 

CRANE SYSTEM 


A. Test Scenarios 

A series of three test cases was used to determine the ability of this 
manipulator device to retrieve an object. In case one, the claw moves to a 
specified location, grips the object if one exists, moves to the drop zone, 
releases the object, and returns home. In case two, the boom and trolley are 
moved to the positions specified. At this point, the claw is lowered until it makes 
contact with the ground or an object. The claw then lifts slightly, grips the object, 
and completes the maneuver. In case three, a relief map of the area 
surrounding the point of initial contact is created. Based on information 
contained in this map and prior knowledge of the target object, either the object 
is located or a new area is searched. Ultimately, either the object is found and 
the maneuver completed or the search is abandoned. 

In case one, the claw moves toward the specified location (6,r,z) d by first 

swinging the boom into position (0 d ) and then moving the trolley (r d ). Once the 
claw is suspended above the desired point, it is lowered to the appropriate height 
(z<j). In the trajectory used in this case, only one motor is in motion at any given 
time. Controller hardware does not limit the system to this serial motion. This 
method is used to avoid undesired contact between the claw ana the 
environment. Due to the crane structure, any motion of boom or trolley requires 
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that no obstacles be present in the space through which the suspended claw 
moves. Keeping the claw retracted until all other motion is complete decreases 
the chances of an unwanted collision with obstacles in the workspace. Upon 
reaching the desired location, the claw is activated. No method of target object 
confirmation is currently in place as part of this system. The vehicle supervisor 
computer is responsible for confirming that the correct object was retrieved using 
some part of its sensor array. The object, having been retrieved, is moved to a 
previously defined point and dropped into a receptacle. The claw is then 
returned to home position. 

The key difference in case two is that the final height of the claw (z) is not 
necessarily the height specified (Zd). When the exact height is unknown by the 
supervisor, a value at the limit or beyond the reachable workspace is used. 
Whenever the claw makes contact with some object before the specified height 
is reached, the claw descent is halted by the master controller. Next, the claw 
suspension cable is retracted a distance of between one-half inch and one inch. 
This claw height above the ground was determined by trial-and-error to be ideal 
for gripping the target object. The claw is then closed and the remainder of the 
maneuver is identical to case one. 

In case three, the specified boom and trolley positions (0, r) d define the 
central vertical axis of a search space instead of the vertical line along which the 
object lies. As in case two, the claw is lowered until it makes contact with the 
workspace. In case three, however, this motion occurs at least at the nine points 
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illustrated in grid 1 of Figure 4-1. The height coordinate (z) at each point is 
measured and is normalized by subtracting the lowest height found. The heights 
are then converted to a number of quarter inches. A typical resulting relief map 
is illustrated in Figure 4-2. Since the object dimensions are assumed known, the 
maximum measured height can be compared with a minimum anticipated object 
height. If the object is determined to exist at one of the search points, then the 
maneuver is completed as in the first two cases. If the object is not found at any 
of the search points, then a new section of the workspace is searched. This 
mapping process is repeated in the pattern shown in Figure 4-1 until either all the 
specified areas are searched or the object is found. 
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Figure 4-2: Typical Relief Map of Minor Grid 


B. Results and Evaluation 

In case one, the object was retrieved if the boom position was within 
approximately 3°, the trolley position was within 0.5 in, and the claw height was 
within 0.5 in of the object center. The primary limitation of this scheme is that the 
specified position provided by the vehicle supervisor must be fairly accurate. 
Precise object location may prove difficult for the vehicle sensors to measure due 
to the small size of the object, the natural terrain background, and the difficulty in 
sensing within 8 in of the vehicle. Actuator output is plotted in Figure 4-3. The 
target position used in this case is +00,+00,-1 80, +07.0, -10.0. 

In case two, the burden of sensing object height is shouldered by the 
manipulator device. The vehicle sensors must still synthesize a fairly accurate 
two dimensional image of the object as viewed from above. While an 
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improvement over case one, the vehicle sensors still limit mission success. The 
target object will typically be sighted before it is within reach of the manipulator. 
The vehicle must then move into position and stop before retrieval can occur. As 
in case one, this close proximity surface may be difficult to map. Actuator output 
is plotted in Figure 4-4. The target position used in this case is +00, +00,- 
180, +07. 0,-1 0.0. Note that the only difference in case one and case two is that 
the claw height did not reach -10 in. Instead, a collision with an obstacle forced 
the claw to stop and grip at approximately -2 in. 

Case three actuator outputs are illustrated in Figure 4-5. The boom angle, 
trolley radius, and claw height all demonstrate the multipoint sounding technique 
used to map the area. As in the previous two cases, the input string was 
+00, +00,-1 80, +07. 0,-1 0.0. In this case, however, the object was actually located 
at +00, +00, -150, +07.0, -02.0. The resulting relief map is illustrated in Figure 4-6. 
In this case, the specified coordinates must only be accurate enough to ensure 
that the object is within the search area. As long as the object is within two 
horizontal inches in the radial direction and four horizontal inches in the 
tangential direction of the specified position, the object can be found and 
retrieved. The ability of the manipulator to map the area and find the object is a 
big advantage. Mission success in retrieving the object is much more likely with 
this scenario. The major limitation of this method is that the maximum resolution 
of the search pattern is limited by the physical dimensions of the claw. Due to 
the large diameter of the closed claw, undesired contact between claw and 
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object sometimes occurs. This unwanted contact can cause the object to move 
to a previously mapped location and be missed in the search or the claw height 
to be inaccurately measured. A future modification of the system would be to 
modify the claw so that contact between the claw and objects not directly 
beneath it would be reduced. 
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Claw (1 =on) 



Figure 4-5: Case 3 Actuator Output 
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Figure 4-6: Case 3 Relief Map 


In all three cases, an intermittent hardware or software error occasionally 
gives grossly inaccurate count values in motor position. This problem has been 
resolved in software by comparing the actual counter value and an estimated 
value. When gross differences occur, the estimate is used by the motor 
controller and the master controller is notified. 
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Chapter Five: CONCLUSIONS AND SUGGESTIONS FOR 

FUTURE WORK 

The manipulator system successfully meets the stated requirements of 
size, weight, and functionality. A clay cylinder measuring 1 .25 in by 1 .25 in was 
used as the target object during system testing. Given an initial boom angle 
within 24° and an initial trolley position within 4 in of the actual values, the 

system will locate and retrieve the target object approximately 80% of the time. 

The search and recovery algorithm eliminates the need for precisely 
measured target coordinates. This feature allows the rover to dedicate its 
sensors to more important data gathering. It also eliminates the need for 
sensors dedicated to surveying the area within 8 in of the vehicle base. The 
vehicle can locate an object at a distance limited only by its sensor arrays, move 
into position near the object, and provide the manipulator with estimated 
coordinates relative to the vehicle. 

The manipulator does occasionally miss the target in its search and 
recovery effort. In these cases, the claw typically causes the object to move into 
an already measured point when sounding the area immediately surrounding the 
object. As a result, the object does not show up in the search map. In these 
situations, the vehicle sensor system will sense the absence of the target object 
and will resubmit the command. This tactic of multiple attempts is usually 
successful. 
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The system is constructed in a modular form that will allow future users to 
optimize the interaction between the vehicle system and the manipulator 
subsystem. Sensors and actuators can be modified to provide new performance 
characteristics. Control software can be altered for testing of new control and 
search algorithms. 

Modification of the end effector could improve system effectiveness in 
three ways. A smaller claw would both allow a higher resolution mapping 
algorithm and reduce unwanted claw/object interaction. This change will allow a 
finer resolution and smaller object search capabilities. Redesigning the fingers 
of the claw could increase the acceptable variation of object size, shape, and 
consistency. 

The master controller software listed in the Appendices neglects the 
effects of platform orientation on actual position. Since the claw is suspended by 
a cable, a change in position of the motor controlling claw height results in claw 
motion along the z axis of the world coordinate system. Any pitch or roll of the 
vehicle will cause the z axis of the manipulator coordinate system to no longer be 
parallel with the z axis of the world coordinate system. Inclusion of these effects 
when calculating desired motor positions based on a specified target coordinates 
would result in increased accuracy in claw placement. 
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A. Master Controller Circuit Diagram 
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B. Motor Controller Circuit Diagram 
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MDTOR CONTROLLER 







C. Master Controller Program Listing 
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MASTER SOFTWARE FOR MARS ROVER MANIPULATOR 


Based on: 
08/19/94 


ctO 


in / sci out ;with sequence input ok 


* 11/07/94 

* 

* 11/08/94 


* 11/10/94 

* 

* 12/01/94 


* 12/03/94 


acia 

- CK.Chao 

ll/ 01 / 94 tiO mstl.a«m Read MAXIN bytea of data 6 echo to motor CFO. 

mast2.asm Run main loop until command received from 
supervisor. Echo c omman d once than return to mai n l oop 
maat3 .asm PA7 commands claw upon receipt of Cl command. 

C (anything besides 1) results in claw off command. Any other 
string is sent out SCI 

added lines in INITPA to disable IRQ on pins 
PA0, PA1, PA2, PA3. 

mast 4 . asm — Modified input command structure to 

address (one byte hex), theta (four ASCII characters 
representing a two byte hex) . , 

masts. asm — Receive and echo output from motor controllers. 

Each motor controller sends the master address («), four 
ASCII characters representing a two byte hex number ( e 
motor position in degrees), two ASCII characters 
representing the controller mode (closed loop (CO) or °P®“ 
loop (Ol, 02, or 03)), and four ASCII characters representing 
the command received by the motor (desired motor positon 
in degrees. 

mast 6 .asm — Rearrange functions to allow for easie 

inclusion of conversion routine. Output printed whenever 
flag (ffbkl, ffbk2 , ffbk3 , f fbkc) set. Input command syntax 
modified to -±pp,±qq. ±ttt, zfcrr.r, ±m.z«. Command *yntax is 
verified after receipt. An error message is written to 
flC j a , nn invalid command is received, 
mast?. asm - Configure PA3 (IC4) as the interrupt pin (on 

falling edge) indicating loss of tension in crane cable. 
Interrupt Service Routine (SLAK_ISR) will be called. 

Modified putaway routine to take claw all the way to e 
top before moving to drop zone, 
mast 8 .asm — Add routines to search around nominal target for 
high spot. After identification, lift at high spot. 

Modify output format to eliminate mode information and 
add 0 as prefix and h as suffix to angle information. This 
will allow Mathcad plot. Add PROTDAT call in ACIA ISR to 
print home position after valid input string to add 
home position to plot. Added routine findit to search 
for object around input ( theta, r). Moved -docmds from main 
fco new routine. 


* 12/12/94 


* 01/03/95 


* 01/09/95 


B96 

equ 

R_DATA 

equ 

CR 

equ 

SPACE 

equ 

LF 

equ 

ACIA 

equ 

PCHAR 

equ 

QCHAR 

equ 

TCHAR 

equ 

RCHAR 

equ 

ZCHAR 

equ 

MAXIN 

equ 

MAXSCI 

equ 

RDPI 

equ 

RZERO 

equ 

ZDPI 

equ 

ZZERO 

equ 


*00110000 

$4000 

$0D 

$20 

$0A 

$1800 

$03 

$03 

$04 

$05 

$05 

PCHAR+QCHAR+TCHAR+RCHAR+ZCHAR+$ 04 
$06 
$51 
$38B 
$74 
$FE78 
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* jump table 


org 

jmp 

#$00ee 

ACIA_ISR 

♦IRQ 

org 

jmp 

#$00e2 

SCI0_ISR 

♦IC3 

org 

jmp 

#$00e5 

SCI1_ISR 

♦IC2 

org 

jmp 

#$00e8 

SCI2_ISR 

♦IC1 

org 

jmp 

#$00d3 

SLAK_ISR 

♦IC4/OC5 


* EEPROM content a 


OR Q del_r * Search pattern constants 

PDB 2 * del_r (half Inches) 

ORG del_theta 

PDB 8 * del_theta (degrees) 


* the variables 



org 

#R_DATA 

rdata 

mb 

MAX IN 

boxl 

rmb 

MAX IN 

box2 

rmh 

MAX IN 

box3 

rmb 

MAXIN 

box4 

rmb 

MAXIN 

box5 

rmb 

MAXIN 

box 6 

rmb 

MAXIN 

box7 

rmb 

MAXIN 

box8 

rmb 

MAXIN 

box9 

rmb 

MAXIN 

cntrl 

rmb 

MAXIN 

cntr2 

rmb 

MAXIN 

cntr3 

rmb 

MAXIN 

cntr4 

rmb 

MAXIN 

cntrS 

rmb 

MAXIN 

cntr6 

rmb 

MAXIN 

cntr7 

rmb 

MAXIN 

cntr8 

rmb 

MAXIN 

cntr9 

rmb 

MAXIN 

hex2 

rmb 

1 

sciO_in 

rmb 

MAXSCI 

scil_in 

rmb 

MAXSCI 

sci2_in 

rmb 

MAXSCI 

detain 

rmb 

1 

ffbkc 

rmb 

1 

ffbkl 

rmb 

1 

f fbk2 

rmb 

1 

££bk3 

rmb 

1 

fcmdc 

rmb 

1 

fcmdl 

rmb 

1 

£cmd2 

rmb 

1 

f cznd3 

rmb 

1 

fstop 

rmb 

1 

pitch 

rmb 

PCHAR 

roll 

rmb 

QCHAR 

theta 

rmb 

TCHAR 

radius 

rmb 

RCHAR 

height 

rmb 

ZCHAR 

thl 

rmb 

2 

th2 

rmb 

2 

th3 

rmb 

2 

th3a 

rmb 

4 
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th3s 

rmb 

2 



temp 

nab 

2 



tmpl6 

rmb 

2 



clawcmd 

rmb 

1 



stphgt 

rmb 

2 



hgtl 

rmb 

2 



hgt2 

rmb 

2 



hgt3 

nab 

2 



hgt4 

rmb 

2 



hgt5 

rmb 

2 



hgt6 

rmb 

2 



hgt7 

rmb 

2 



hgt8 

rmb 

2 



hgt9 

nab 

2 



target 

rmb 

1 



try 

rmb 

1 



whole_in 

rmb 

2 


half_in 


nab 

2 



rmb 

40 



stack 

rmb 

1 



SH7TREQ 

RMB 

2 input 

, shift 

register 

TMP1 

RMB 

1 




org 

$4500 


* eventually B700 

del_r 


rmb 

2 


del__theta 

rmb 

2 



* the code starts here. 


org $2000 

jrap START 


* THE FUNCTION LIBRARY 


* bin2hex — Separates each character of a hex number and calls outhex 


bin2hex pshb 
psha 
lsra 
lsra 
lsra 
lsra 

anda #*00001111 

j sr outhex 

pula 

anda **00001111 

j sr outhex 

zb2hex pulb 
rts 


* getsci — if a 

* retrieves 


character has been received on sci port, 
and places in accumulator A. 


this character 


getsci 

Idx 

#REGBAS 


ldab 

SCSR/ X 


bitb 

*$20 


beq 

getsci 


Idaa 

SCDR , X 

zg_sci 

rts 



* if RDRF is 0 then wait 


* HEXBIN(a) - Convert the ASCII character in A 

* to binary and shift into shftreg. 
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HEXBIN 

PSHA 

'**** 



PSHB 




PSHX 



• 

JSR 

UPCASE 

convert to upper case 


CMPA 

#’0* 



BLT 

HEXNOT 

jump if a < $30 


CMPA 

# * 9 ' 



BLE 

HEXNMB 

jump if 0-9 


CMPA 

# ’ A' 



BLT 

HEXNOT 

jump if $39> a <$41 


CMPA 

#'F* 



BGT 

HEXNOT 

jump if a > $46 


ADDA 

#$9 

convert $A-$F 

HEXNMB 

ANDA 

#$0F 

convert to binary 


LDX 

#SHFTREG 



LDAB 

*4 


HEXSHFT 

ASL 

1,X 

2 byte shift through 


ROL 

o,x 

carry bit 


DECB 




BGT 

HEXSHFT 

shift 4 times 


ORAA 

1,X 



STAA 

lfX 



BRA 

HEXRTS 


HEXNOT 

nop 



* 

INC 

TMPl 

indicate not hex 

HEXRTS 

PULX 




PULB 

PULA 

RTS 


* onacia — initializes acia 

port 


onacia: 

ldx 

cli 

#REGBAS 


* enable IRQ (clear I in CCR) 


Idea 

ora 

OPTION, X 
#*00100000 


* set IRQ to recognize falling edge 

* 

staa 

ldaa 

staa 

ldaa 

OPTION, X 

#$03 

ACIA 

#*00010110 


* master reset of ACIA 

* ACIA is at $1800 

* cr4,cr3, cr2 - 101 : 8bit Istop bit 

* crl,cr0 = 10 : d 64 (IRQ enabled) 


ora 

#*10000000 


* enable IRQ 


staa 

ACIA 



zonae la 

rts 




* onsci 

»<** 

— initializes sci port 


onsci: 

ldx 

# REG BAS 




ldaa 

staa 

ldaa 

staa 

ldaa 

staa 

#B96 
BAUD, X 
#*00001100 
SCCR2 , X 
#*00000000 
SCCR1,X 


* set baud rate 

*m MAS 00001100 made 00101110 

* enable transmit 6 receive 
*m was 00000000 made 00001000 

* set data is 8-bit mode 

zonsci 

rts 




* out hex — converts hex to 

ASCII 

and transmits out ACIA port 

outhex 

empa 

#10 




bge 

ge2A 




adda 

#$30 




j-r 

put acia 




bra 

zo_hex 



ge2A: 

adda 

j*r 

#$37 

putacia 
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zo_hex rts 


* outhexs — converts hex to ASCII and transmits out SCI port 


outhexs crmpa #10 

bge go 2 As 

adds #$30 

jsr putsci 

bra zo_hexs 

ge2As: adda #$37 

jsr putsci 

zo_hexs rts 


* putacia -- puts byte in accumulator A out acia port. 


putacia ldab ACIA 

bitb #$02 

beq putacia 

anda #$ff 

staa ACIA+1 

zp_acia rts 


* putsci -- puts byte in accumulator A out sci port. 


putsci ldx #REGBAS 

ldab SCSR,X 

bitb #$80 

beq putsci 

anda #$£f 

staa SCDR, X 

zp_sci rts 


* scib2h -- Separates each character of a hex number and calls outhexs 


scib2h psha 
lsra 
lsra 
lsra 
lsra 
anda 

#*00001111 

jsr 

outhexs 

pula 

anda 

#*00001111 

jsr 

outhexs 

zscib2h rts 



* S I/O DOWN — 

kills time when 

S I/O DOWN psha 


pshb 


ldad 

#$FFFT 

SI/01 subd 

#$0001 

bne 

SLOl 

pulb 


pula 


rts 



* THE FUNCTIONS SPECIFIC TO MASTER OPERATION 


★ AS C 2 HEX -- Converts an ASCII character (0-9) in accumulator B to a 
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* if TDRE is 0 loop back to putsci 

* (not ready to be sent) 


hex n umb er - This number is returned In accumulator B 


AS C 2 HEX cmpb #'0* 

bit za2h 

cmpb # 1 9 ' 

bgt za2h 

subb #$30 

za2h rts 


* checkin — Read input atring and verify ayntax 


checkin ldy 

#R_DATA 

Idaa 

0,Y 

jsr 

iaaign 

cmpb 

#•1' 

bne 

synerrl 

ldaa 

1#Y 

j»r 

isint 

cmpb 

#'l' 

bne 

synerrl 

ldaa 

2,Y 

jsr 

isint 

cmpb 

#•1' 

bne 

synerrl 

ldaa 

3 # Y 

jar 

iacomsp 

cmpb 

#'l* 

bne 

synerrl 

ldaa 

4,Y 

jsr 

issign 

cmpb 

#'l' 

bne 

synerrl 

ldaa 

5 , Y 

jsr 

isint 

cmpb 

#•1' 

beq 

set2 

synerrl jmp 

synerr 

set2 ldaa 

6rY 

jar 

isint 

cmpb 

#•1' 

bne 

synerrl 

ldaa 

7,Y 

jsr 

iscomap 

cmpb 

#’l' 

bne 

synerrl 

ldaa 

8,Y 

jsr 

issign 

cmpb 

#•1* 

bne 

synerrl 

ldaa 

9 , Y 

jsr 

isint 

cmpb 

# ' 1 ' 

bne 

synerr2 

ldaa 

10, Y 

jsr 

isint 

cmpb 

# ' 1 • 

bne 

synerr 2 

ldaa 

11/ Y 

jsr 

isint 

cmpb 

#'1* 

bne 

synerr 2 

ldaa 

12, Y 

jsr 

iscomap 

cmpb 

#'l' 

beq 

set3 
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synerr2 jmp 


gynerr 


set3 

ld&a 

13, Y 


jar 

isaign 


cznpb 

#•!■ 


bne 

synerr3 


ldaa 

14, Y 


jar 

isint 


cmpb 

#'1* 


bne 

aynerr3 


ldaa 

15, Y 


jar 

ialnt 


cmpb 

#•1* 


bne 

aynerr3 


ldaa 

16, Y 


jar 

isdec 


cmpb 

#•1* 


bne 

aynerr3 


ldaa 

17, Y 


jar 

ialnt 


cmpb 

• •l* 


bne 

aynerr3 


bra 

aet4 

aynerr3 

jmp 

gynerr 

set 4 

ldaa 

18, Y 


jar 

iscomap 


cmpb 

#'l* 


bne 

gynerr 


ldaa 

19, Y 


jar 

isaign 


cmpb 

#•1* 


bne 

synerr 


ldaa 

20, Y 


jar 

iaint 


cmpb 

#'l* 


bne 

synerr 


ldaa 

21, Y 


jar 

iaint 


cmpb 

#■1* 


bne 

synerr 


ldaa 

22, Y 


jar 

isdec 


cmpb 

• •l* 


bne 

synerr 


ldaa 

23, Y 


jar 

isint 


cmpb 

# ' 1 ' 


bne 

synerr 

noerr 

ldaa 

#•!• 


ataa 

datain 


bra 

zcheck 

synerr 

jar 

wrerrO 


ldaa 

#'0' 


ataa 

datain 


ataa 

clawcxnd 


bra 

zcheck 


z check nop 
rta 





beq 

doeml 



jar 

outcxndl 


waitl 

ldaa 

ffbkl 



cmpa 

#•1' 



beq 

adocmds 



bra 

waitl 


adocmda 

jsr 

doout 


doeml 

ldaa 

f cmd2 



cxnpa 

#'0* 



beq 

doem2 



jar 

outcmd2 


wait2 

ldaa 

ffbk2 



cmpa 

#•1' 



beq 

bdocmds 



bra 

wait2 


bdocmds 

jar 

doout 


doem2 

nop 




ldaa 

£cmd3 



cmpa 

#'0' 



beq 

doem3 



jar 

outcmd3 


wait3 

ldaa 

ffbk3 



cmpa 

#’l* 



beq 

cdocmda 



ldaa 

fstop 



cmpa 

#'1 # 



bne 

waits 



ldaa 

#'0* 



staa 

f atop 



ataa 

f cmdl 



staa 

f cznd2 



bra 

bdocmds 


cdocmdfl 

> jar 

doout 


doeza3 

jar 

grip 



jsr 

doout 



bset 

TFLGlrX 

o 

o 

o 

H 

O 

o 

o 

o 

*: 


baet 

TMSKl,X 

#*00001000 

ddocmds ldaa 

#*0' 



staa 

detain 



zdocmds rt 8 


* Clear IC4 flag 

* Enable IC4 interrupt 


* docmdo2 — Move manipulator to commanded position in 


reverse order 


docznda2 

ldaa 

£cmd3 


cznpa 

#'0' 


beq 

dooms 1 


jar 

outcmd3 

wait21 

ldaa 

ffbk3 


cmpa 

#'l' 


beq 

adocmd2 


bra 

wait21 

adocmd2 

jar 

doout 

doem21 

ldaa 

f cmd2 


cmpa 

#• o • 


beq 

doem22 


jsr 

outcmd2 

wait22 

ldaa 

ffbkS 


cmpa 

#■1' 


beq 

bdocznd2 


bra 

wait22 

bdocmdS 

jar 

doout 

doemS 2 

ldaa 

f cmdl 


cmpa 

#•0' 


beq 

doom! 3 


jsr 

outcmdl 

wait23 

ldaa 

ffbkl 


cmpa 

#•1' 


beq 

cdocmdS 
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bra 

wait 2 3 

cdocmd2 

j*r 

doout 

doem23 

jsr 

grip 


j*r 

doout 


ldaa 

#•0* 


staa 

datain 

zdocmd2 

rts 



* doout — write output to icreen is necessary. 


doout 


zdoout 


ldaa 

ffbkl 

suba 

#$30 

adda 

ffbk2 

suba 

#$30 

adda 

f£bk3 

suba 

#$30 

adda 

ffbkc 

suba 

#$30 

cznpa 

#$00 

beq 

zdoout 

ldaa 

#*0* 

staa 

ffbkl 

staa 

f fbk2 

staa 

f fbk3 

staa 

ffbkc 

jar 

PRNTDAT 

rts 



findit — Search for object around given approximate position. Algorithm 
assumes that the beginning (input) point is the beginning of 
the search. This point lies at the center of an imaginary 
tic-tac-toe board. All nine points are sounded and their 
heights (th3 values) are stored in hgtl-hgt9. If the center 
point is del_z above the perimeter, then the claw makes a 
grab at the center point. If any of the perimeter squares 
are del_z taller than the center, 

that point becomes the new center and the search pattern is 
run again. If two perimeter points are both equal and 
taller than the center, then... This process repeats 
until . • . 

If (hgt5-del_z > all other hgt values) 

Lift at center 
Else if (hgti 
Case 2: 

cntri (i-l:9) are the centers of imaginary tic-tac-toe 
boards that will be used in the search for the object, 
boxi (i*l : 9 ) are the individual squares of the particular 
tic-tac-toe board currently being searched. 


findit 

psha 

pshb 

pshx 

pshy 



ldy 

#rdata 


ldx 

#cntrl 

svcntrl 

ldaa 

0 , Y 


staa 

iny 

inx 

0,X 


cpy 

#r da t a + #MAXIN 


bit 

svcntrl 


ldy 

#rdata 


ldx 

#cntr2 

svcntr2 

ldaa 

0, Y 


Save rdata to cntri for later use. 


Save rdata to cntr2 for later use. 
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staa 

iny 

inx 

cpy 

bit 

ldy 

Idx 

svc at r 3 ldaa 
staa 
iny 
inx 
cpy 
bit 

ldy 

ldx 

s vent r 4 ldaa 
staa 
iny 
inx 
cpy 
bit 

ldy 

ldx 

sventrS ldaa 
staa 
iny 
inx 
cpy 
bit 

ldy 

ldx 

sventrS ldaa 
staa 
iny 
inx 
cpy 
bit 


0,X 

#rdata+#MAXIN 

svcntr2 

#rdata 

#cntr3 

0,Y 

o,x 

#rdata+#MAXIN 
s vent r 3 

frdata 
#cntr4 
0 , Y 
0,X 


frdata+fMAXIN 

svcntr4 

frdata 

#cntr5 

0,Y 

0,X 

frdata+fMAXIN 

sventrS 

#rdata 

#cntr6 

0,Y 

0,X 

frdata+fMAXIN 

svcntr6 


* Save rdata to cntr3 for later use. 


* Save rdata to cntr4 for later use 


* Save rdata to cntr5 for later use 


* Save rdata to cntrS for later use. 


ldy 

ldx 

svcntr7 ldaa 
staa 
iny 
inx 
cpy 
bit 


#rdata 

#cntr7 

0,Y 

o,x 


* Save rdata to cntr7 for later use. 


frdata+fMAXIN 

svcntr7 


ldy 

ldx 

svcntr8 ldaa 
staa 
iny 
inx 
cpy 
bit 


frdata 
#cntr8 
0 # Y 
0,X 


* Save rdata to cntr8 for later use 


#rdata+#MAXIN 

svcntr8 


ldy 

ldx 

sventrS ldaa 
staa 
iny 
inx 
cpy 
bit 


#rdata 

#cntr9 

0,Y 

0,X 


* Save rdata to cntr9 for later use. 


#rdata+#MAXIN 

svcntr9 
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jar 

pattbig 


Idaa 

# ' 1 ' 


staa 

try 

search 

Idy 

#rdata 


ldaa 

try 

tryl 

cznpa 

t'l' 


bne 

try 2 


Idx 

#cntrl 


bra 

strdata 

try 2 

cmpa 

t'2* 


bne 

try3 


Idx 

#cntr2 


bra 

strdata 

try3 

cmpa 

#'3* 


bne 

try4 


Idx 

#cntr3 


bra 

strdata 

try 4 

cznpa 

# ' 4 1 


bne 

try5 


Idx 

#cntr4 


bra 

strdata 

try 5 

cmpa 

#'5' 


bne 

try6 


Idx 

#cntr5 


bra 

strdata 

tryfi 

cmpa 

t ' 6 1 


bne 

nomore 


Idx 

#cntr6 


bra 

strdata 

nomore 

jmp 

giveup 

st rdata 

ldaa 

0,X 


staa 

iny 

inx 

0,Y 


cpy 

trdata+tMAXIN 


bit 

strdata 


Idy 

#rdata 


Idx 

#boxl 

svrdatl 

ldaa 

0,Y 


staa 

iny 

inx 

0,X 


cpy 

trdata+tMAXIN 


bit 

svrdat 1 


Idy 

trdata 


Idx 

#box2 

svrdat2 

ldaa 

0, Y 


staa 

iny 

inx 

0,X 


cpy 

trdata+tMAXIN 


bit 

svrdat 2 


Idy 

trdata 


Idx 

#box3 

svrdat3 

ldaa 

0, Y 


staa 

iny 

inx 

0,X 


cpy 

trdata+tMAXIN 


bit 

svrdat 3 


Idy 

trdata 


Idx 

tbox4 

svrdat4 

ldaa 

0,Y 


* Modify cntr values to show spread. 

* Set cntr to search around first 


* Save cntri to rdata to be searched. If 

* all 9 grids have been searched, then 

* give up . 


* six square case 


* Save rdata to boxl for later use. 


* Save rdata to box2 for later use . 


* Save rdata to box3 for later use. 


* Save rdata to box4 for later use. 
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staa 0 , X 

iny 

inx 

cpy #rdata+#MAXIN 

bit svrdat 4 


ldy 

ldx 

svrdat 5 ldaa 
ataa 
iny 
inx 
cpy 
bit 


#rdata * Save rdata to box5 for later use. 

#box5 

0,Y 

0,X 


#r dat a + #MAXIN 
svrdat 5 


ldy 

ldx 

svrdat 6 ldaa 
staa 
iny 
inx 
cpy 
bit 

ldy 

ldx 

avrdat7 ldaa 
ataa 
iny 
inx 
cpy 
bit 

ldy 

ldx 

svrdat 8 ldaa 
staa 
iny 
inx 
cpy 
bit 

ldy 

ldx 

svrdat 9 ldaa 
staa 
iny 
inx 
cpy 
bit 

jar 

ldaa 

staa 

j-r 

ldy 

ldx 

ldrdatl ldaa 
staa 
iny 
inx 
cpy 
bit 

ldaa 

staa 

jar 


#rdata 

#box6 

0,Y 

0,X 


#rdata+#MAXIN 
svrdat € 

#rdata 

#box7 

0,Y 

0,X 


#rdata+#MAXIN 
svrdat 7 

#rdata 

#box8 

0,Y 

0,X 


#rdata+#MAXIN 
svrdat 8 

#rdata 
#box9 
0# Y 
0 f X 


#rdata+#MAXIN 
svrdat 9 

pattern 

#• 1 * 

clawcmd 

docmds 

#rdata 

#boxl 

0,X 

0,Y 


#rdata+#MAXIN 

ldrdatl 

#* 1 * 

datain 

malcecmd 


* Save rdata to box 6 for later use. 


* Save rdata to box7 for later use. 


* Save rdata to box8 for later use. 


* Save rdata to box9 for later use . 


* Modify box positions to spread pattern 

* Close claw 


* Save boxl to rdata for sounding. 


* Sound boxl 
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jar 

docmda 

ldd 

th3e 

std 

hgtl 

jar 

gotop 

ldy 

frdata 

ldx 

#box2 

ldrdat2 ldaa 

o,x 

staa 

iny 

inx 

0 r Y 

cpy 

#rdata+#MAXIN 

bit 

Idrdat 2 

ldaa 

# * 1 ' 

staa 

datain 

jar 

aakecmd 

jar 

docmda 

ldd 

th3s 

std 

hgt2 

jar 

gotop 

ldy 

#rdata 

ldx 

#box3 

IdrdatB ldaa 

0 r X 

staa 

iny 

inx 

0/ Y 

cpy 

#rdata+#MAXIN 

bit 

Idrdat 3 

ldaa 

#•1' 

staa 

datain 

jar 

mafcecmd 

jar 

docznds 

ldd 

th3s 

std 

hgt3 

jsr 

gotop 

ldy 

#rdata 

ldx 

#box6 

ldrdat6 ldaa 

0# X 

staa 

iny 

inx 

0 f Y 

cpy 

#rdata+#MAXIN 

bit 

Idrdat 6 

ldaa 

#•1* 

staa 

datain 

jar 

makecmd 

jar 

docmds 

ldd 

th3s 

std 

hgt6 

jar 

gotop 

ldy 

#rdata 

ldx 

#boxS 

ldrdat5 ldaa 

0 f X 

staa 

iny 

inx 

0 , Y 

cpy 

#rdata+#MAXIN 

bit 

Idrdat 5 


* Save bo x2 to rdata Cor sounding. 


* Sound box2 


* Save box3 to rdata for sounding. 


* Sound box3 


* Save box6 to rdata for sounding. 


* Sound box6 


* Save box5 to rdata for sounding. 


ldaa # ' 1 ' * Sound box5 

staa datain 

j sr makecmd 

jar docmda 
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Idd 

std 

jar 

ldy 

ldx 

1 dr dat 4 ldaa 
staa 
iny 
inx 
cpy 
bit 

ldaa 

staa 

jar 

jar 

Idd 

std 

jar 

ldy 

ldx 

ldrdat7 ldaa 
staa 
iny 
inx 
cpy 
bit 

ldaa 

staa 

jar 

jar 

Idd 

std 

jar 

ldy 

ldx 

ldrdatd ldaa 
staa 
iny 
inx 
cpy 
bit 

ldaa 

staa 

jar 
j sr 
Idd 
std 
jar 

ldy 

ldx 

ldrdat9 ldaa 
staa 
iny 
inx 
cpy 
bit 

ldaa 

staa 

jar 

jar 

Idd 


th3s 
hgt5 
got op 

#rdata 

#box4 

0,X 

0,Y 


#r dat a + #MAXIN 
ldrdat4 

#• 1 * 

datain 

makecmd 

docmds 

th3s 

hgt4 

got op 

#rdata 

#box7 

0,X 

0,Y 


#rdata+#MAXIN 

ldrdat7 

#'l' 

datain 

makecmd 

docmds 

th3s 

hgt7 

got op 

#rdata 
#box8 
0,X 
0 r Y 


#rdata+#MAXIN 
1 dr dat 8 

#’l' 

datain 

maJcecmd 

docmds 

th3s 

hgt8 

gotop 

#rdata 
#box9 
0,X 
0 , Y 


#rdata+#MAXXN 
1 dr dat 9 

#'l' 

datain 

makaczrkd 

docmds 

tb3s 


* Save box4 to rdata for sounding. 


* Sound box4 


* Save box7 to rdata for sounding. 


* Sound box7 


* Save box8 to rdata for sounding. 


* Sound box8 


* Save box9 to rdata for sounding. 


* Sound box9 
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std 

hgt 9 


gotop 



ldd 

hgtl 


cpd 

hgt2 


ble 

sortl 


ldd 

hgt 2 

sortl 

cpd 

hgt 3 


ble 

sort2 


ldd 

hgt 3 

sort 2 

cpd 

hgt 4 


ble 

sort3 


ldd 

hgt 4 

sort3 

cpd 

hgt 5 


ble 

sort4 


ldd 

hgt 5 

sort 4 

cpd 

hgt 6 


ble 

sort5 


ldd 

hgt 6 

sorts 

cpd 

hgt 7 


ble 

sort 6 


ldd 

hgt 7 

sorts 

cpd 

hgt 8 


ble 

sort7 


ldd 

hgt 8 

sort7 

cpd 

hgt 9 


ble 

sorted 


ldd 

hgt 9 

sorted 

std 

trip 1 6 


ldd 

hgtl 


subd 

tmpl6 


std 

hgtl 


ldd 

hgt2 


subd 

tmplS 


std 

hgt 2 


ldd 

hgt 3 


subd 

tmpl6 


std 

hgt 3 


ldd 

hgt 4 


subd 

tmpl6 


std 

hgt 4 


ldd 

hgt 5 


subd 

tmplS 


std 

hgt 5 


ldd 

hgt 6 


subd 

tmpl6 


std 

hgt 6 


ldd 

hgt 7 


subd 

txnpl6 


std 

hgt 7 


ldd 

hgt 8 


subd 

tmpl6 


std 

hgt 8 


ldd 

hgt 9 


subd 

tmplS 


std 

hgt 9 


nop 



ldd 

#ZDPI 


ldx 

idiv 

pshx 

pula 

#4 


pulb 

std 

tmpl6 


ldd 

hgtl 


ldx 

idiv 

tmpl6 


* Search all values of hgt to find the lowest 

* value. Subtract this value from each 

* hgt. 


* Integer divide each hgt by ZDPI/4 (1/4") 
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pshx 

pula 

pulb 

std 

hgtl 

ldd 

hgt2 

ldx 

taplS 

idiv 

pshx 

pula 

pulb 

std 

hgt2 

ldd 

hgt3 

ldx 

tap 16 

idiv 

pshx 

pula 

pulb 

std 

hgt3 

ldd 

hgt4 

ldx 

t.mpl6 

idiv 

pshx 

pula 

pulb 

std 

hgt4 

ldd 

hgtS 

ldx 

txnpl6 

idiv 

pshx 

pula 

pulb 

std 

hgt5 

ldd 

hgt6 

ldx 

tapl6 

idiv 

pshx 

pula 

pulb 

std 

hgt6 

ldd 

hgt7 

ldx 

taplS 

idiv 

pshx 

pula 

pulb 

std 

hgt7 

ldd 

hgt8 

ldx 

taplS 

idiv 

pshx 

pula 

pulb 

std 

hgt8 

ldd 

hgt9 

ldx 

taplS 

idiv 

pshx 

pula 

pulb 

std 

hgt9 


* Decide whether to: 

* (1) Lift at a box within grid. 

* (2) Make another location the center of new search pattern. 

* (3) Give up. 


ldaa 

staa 


#' 1 * 
target 
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tall! 


tall2 


tall3 


tall4 


tallS 


tails 


tall7 


tall8 


pickup 


svrdat 


ldd hgtl 

cpd hgt2 

bge talll 

ldaa # ' 2 ' 

staa target 

ldd hgt2 

cpd hgt3 

bge tall2 

ldaa # * 3 • 

fltaa target 

ldd hgt3 

cpd hgt4 

bge tall 3 

ldaa # 1 4 ' 

staa target 

ldd hgt4 

cpd hgt5 

bge tall 4 

ldaa # 1 5 ' 

staa target 

ldd hgt5 

cpd hgtS 

bge tall5 

ldaa # ' 6 ‘ 

staa target 

ldd hgtS 

cpd hgt7 

bge tall6 

ldaa # ' 7 1 

staa target 

ldd hgt7 

cpd hgt8 

bge tall7 

ldaa # ' 8 ' 

staa target 

ldd hgt8 

cpd hgt9 

bge tall8 

ldaa * • 9 1 

staa target 

ldd hgt9 

cpd #3 

bgt pickup 

ldaa try 

inca 

staa try 

jznp search 

nop 

ldy #rdata 

ldaa #MAXIN 

ldab target 

subb #$30 

subb #1 

mil 

addd #boxl 

pshb 

pflha 

pulx 

ldaa 0,X 

■taa 0,Y 

iny 
inx 

cpy #rdata+#MAXIN 

bit svrdat 

j sr wrbox 

j sr flhowmap 


* Search all values of hgt to find the 

* greatest value of hgt. Set target ■ 

* the box number of the first occurence 

* of the greatest value. 


* Save target box to rdata for pickup. 

* Convert target to hex, subtract 1, 

* multiply by MAXIN, Add boxl address, 

* store in X. 


* Print search points 

* Print map 
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Idea 

#'0' 

* Open claw to prepare for pickup 

staa 

clawcmd 


jsr 

docxnds 


ldaa 

#-1* 

* Pick up object 

staa 

clawcmd 


staa 

datain 


jsr 

makecmd 


jsr 

docmds 


giveup j sr 

puly 

pulx 

pulb 

pula 

rts 

got op 



* gohoma — Return to home position 


homept 

** + 

fee 

• +00,+00, +000, +07.0, +03 .0' 

gohoma 

ldy 

#R_DATA 


ldx 

#homept 


ldaa 

#'l' 


staa 

fcmdl 


staa 

f cmd2 


staa 

£cmd3 


staa 

femde 


ldaa 

# • 0 ’ 


staa 

clawcmd 

gohomel 

ldaa 

0,X 


staa 

0,Y 

* 

jsr 

putacia 


inx 



iny 



cpy 

#R_DATA+#MAXIN 


bit 

gohomel 

* 

ldaa 

#CR 

* 

jsr 

putacia 


jsr 

checkin 


jsr 

makecmd 


jsr 

doemds2 

* 

jsr 

doout 


rts 


* gotop 

*** 

-- Take 

fr * * 

claw from current position all the way to the top. 

gotop 

psha 



pshb 



pshx 



pshy 

ldaa 

#> 1 * * Take claw to top 


staa 

fcmdl 


staa 

fcmd2 


staa 

fcmd3 


staa 

f erode 


ldaa 

#•!■ 

*c 

staa 

clawcmd 


ldx 

#R_DATA+#MAXIN-#ZCHAR * Note +09.0 is out of range 


ldaa 

£» i * and forces the claw all the way up. 


staa 

inx 

0,X 


ldaa 

#'0* 


staa 

inx 

0,X 

* 

ldaa 

gigi * Temporarily make position 00.0 


ldaa 

#'0* 


staa 

o,x 
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inx 


ldaa 

#* ** .' 

■taa 

inx 

0,X 

ldaa 

# ' 0 1 

staa 

o,x 

jar 

checkin 

jsr 

makecmd 

jsr 

puly 

pulx 

pulb 

pula 

rt* 

docmds2 


* grip — Activate* or deactivate* 


grip 

ldx 

#REGBAS 



ldaa 

clawcmd 



cxnpa 




beg 

clawon 


clawof f 

bclr 

PORTA, X 

#*10000000 


bra 

zgrip 


clawon 

b*et 

PORTA, X 

#*10000000 

zgrip 

ldaa 

#■!■ 



staa 

ffbkc 



rt s 




claw 

* If '1* then turn claw on 

* Otherwise turn claw off 

* return 


* inacia -- read* MAXIN characters in ACIA. Store* at R_DATA. 


inacia jsr 

syntax 

ldaa 

#'0' 

staa 

datain 

ldaa 

#'l' 

*c staa 

clawcmd 

ldy 

#R_DATA 

ldab 

ACIA 

bitb 

#$01 

beq 

zinacia 

READIT ldaa 

ACIA+1 

anda 

#$ff 

staa 

0,Y 

jsr 

putacia 

iny 


cpy 

#R_DATA+#MAXIN-#$01 

bis 

GETNEXT 

ldy 

#R_DATA 

bra 

zinacia 

GETNEXT ldab 

ACIA 

bitb 

#$01 

beq 

GETNEXT 

bra 

READIT 

z inacia nop 


ldaa 

#CR 

jsr 

putacia 

rts 



* close claw when position reached 

* read ACIA status register 

* check LSB, if 1, then new character 

* if no new character, return 

* else read character one 

* this changes CCR, not needed here 

* store charater one 

* echo character 

* increment character pointer 

* compare pointer to max pointer 

* if not all in, goto GETNEXT 

* else reset character pointer 

* always return 

* read ACIA status register 

* check LSB, if 1# then new character 

* repeat until new character in 

* read and save character 


* return 


* init — initial process for the program. Call this function in the 

* first step of the main program. 


init Idaa #SPACE 

j sr putacia 

jsr putacia 

★ 

ldx #REGBAS 
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** Without these three lines, 
** approximately 10 characters 
** of nonsense print prior to 
** the first outcxnd. ????????? 


zinit 


ldy 

j«r 

d«r 

j«r 

rts 


#R_DATA 

onacia 

onsci 

INITPA 

INXVAR 


* INITPA — initializes PORT A (68HC11K9) 


**** 

ldx 

#REGBAS 



ldaa 

#*10000100 



staa 

PACTL, X 



ldaa 

#5601110000 

* 

P0: 000, PI: 001, P2 : 010, P3 : Oil 

staa 

PORTA, X 

* 

P4 : 100, P5 : 101, P6 : 110, P7 : 111 

ldaa 

#5600001111 

* 

reset interrupt flags IC4-IC1 

staa 

TFLG1, X 



ldaa 

#5600001111 

* 

Enable ICi interrupts 

staa 

THSKlrX 



bset 

TCTL2 , X #5610101010 

* 

Interupt ICi on falling edge 

rts 





INIVAR — Initializes variables 


ldy 

#R_DATA 

ldaa 

#SPACE 

staa 

0,Y 

iny 

cpy 

#R_DATA+ #MAXIN- #$01 

bis 

INIVAR1 

ldy 

#R_DATA 

ldaa 

#*0* 

staa 

fcmdl 

staa 

fcad2 

staa 

fcmd3 

staa 

f czndc 

staa 

ffbkl 

staa 

f £bk2 

staa 

ffbk3 

staa 

ffbkc 

staa 

f stop 

staa 

clawcmd 

ldd 

#$0000 

std 

thl 

std 

th2 

std 

th3 

ldaa 

#'X’ 

ldy 

#sci0_in 

staa 

0,Y 

iny 

cpy 

#sci0_in+#MAXSCI-#$01 

bis 

initsO 

ldy 

#scil_in 

staa 

0 , Y 

iny 

cpy 

#scil_in+#MAXSCI-#$01 

bis 

initsl 

ldy 

#sci2_in 

staa 

0 , Y 

iny 

cpy 

#sci2_in+#MAXSCI-#$01 

bis 

inits2 

ldaa 

#0 

staa 

hgtl 

staa 

hgtl+1 

staa 

hgt2 

staa 

hgt2+l 

staa 

hgt3 
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■taa 

hgt3+l 

staa 

hgt4 

■taa 

hgt4+l 

■taa 

hgt5 

staa 

hgt5+l 

staa 

hgt6 

staa 

hgt6+l 

staa 

hgt7 

staa 

hgt7+l 

staa 

hgt8 

staa 

hgt8+l 

staa 

hgt9 

staa 

hgt9+l 

ZINTVAR rts 


* i scorns p — 

Writes 1 1 1 

* 

• , 1 or 

i scorns p ldab 

#•1’ 

cmpa 

#'r * 

beq 

ziscom 

cmpa 

#• 1 

beq 

ziscom 

ldab 

#*0’ 

ziscom rts 



accumulator B if character in accumulator X ia 
else writes *0*. 


1*111*-- Write. •!' to accumulator B if character in accumulator A i. 


* 


1 . 1 else writes 

isdec 

ldab 

#•1' 


cmpa 

#• . ‘ 


beq 

zisdec 


ldab 

#•0* 

zisdec 

rts 



isint — Write* ' 1' to 


* 

■1', '2' 

* 

writes 

isint ldab 

#■!• 

cmpa 

#'0* 

beq 

zisint 

cmpa 

# 1 1 * 

beq 

zisint 

cmpa 

# 1 2 1 

beq 

zisint 

cmpa 

#‘3» 

beq 

zisint 

cmpa 

#•4’ 

beq 

zisint 

cmpa 

#'5' 

beq 

zisint 

cmpa 

#■6* 

beq 

zisint 

cmpa 

#•7’ 

beq 

zisint 

cmpa 

# * 8 ’ 

beq 

zisint 

cmpa 

#■9' 

beq 

zisint 

ldab 

#«0’ 

zisint rts 


* issign -- 

Writes • 1 1 

* 

■ + • or 


accumulator B if character in 

• 3 ', ' 4 ', ' 5 ’ , ' 6 1 ' ‘ 7 ’ ' * 8 ’ 


to accumulator B if character 
•-* else write* 'O’ . 


accumulator A i* 

, * 9 1 , ’O’, el*e 


in accumulator A ia 
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issign 


ldab 

#'l* 

cmpa 

#' + ' 

beq 

zissign 

cmpa 

♦ '-' 

beq 

zissign 

cmpa 

#' ' 

beq 

zissign 

ldab 

♦ 'O' 


zissign rts 


makecmd — Convert input string into motor commands. 

******** * 

* if data in no good, quit 


makecmd ldaa 
cmpa 
beq 
jmp 

begmake nop 


datain 
#' 1 ' 
begmake 
zmakcmd 


* parse received string into 

* individual strings 


getp 


getq 


getth 


ldx 

♦R_DATA 

ldy 

♦pitch 

ldaa 

o,x 

staa 

0,Y 

ixuc 

iny 

cpy 

#pitch+#PCHAR 

bit 

getp 

inx 

ldy 

♦roll 

ldaa 

0,X 

staa 

0,Y 

inx 

iny 

cpy 

♦roll+^QCHAR 

bit 

getq 

inx 

ldy 

♦theta 

ldaa 

0,X 

staa 

0,Y 

inx 

iny 

cpy 

♦theta+^TCHAR 

bit 

getth 

inx 

ldy 

♦radius 

ldaa 

0,X 

staa 

0,Y 

inx 

iny 

cpy 

♦radius+^RCHAR 

bit 

getrad 

inx 

ldy 

♦height 

ldaa 

o,x 

staa 

0,Y 

inx 

iny 

cpy 

♦height+#ZCHAR 


* store string in pitch 


* store string in roll 


store string in theta 


* store string in radius 


* store string in height 
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bit gethght 


makethl 

ldab 

theta+#TCHAR-#$0 1 


jsr 

AS C 2 HEX 


Idea 

#$00 


std 

thl 


ldab 

theta+#TCHAR-#$ 02 


jsr 

AS C 2 HEX 


Idaa 

xnul 

#10 


addd 

thl 


std 

thl 


ldab 

theta+#TCHAR-#$ 03 


jsr 

ASC2HBX 


ldaa 

xnul 

#100 


addd 

thl 


std 

thl 


ldab 

theta 


cxnpb 

#’-' 


beq 

mkthla 


ldd 

#$0000 


subd 

thl 


std 

thl 

mkthla 

ldaa 

#*1‘ 


staa 

fcmdl 

maketh2 

ldab 

radiu»+#RCHAR- #$ 0 1 


cxnpb 

#•5' 


bit 

mkth2a 


ldd 

#RDPI 


ldx 

idiv 

pshx 

pula 

pulb 

#$0002 


bra 

mkth2b 

mkthla 

ldd 

#$0000 

mkth2b 

std 

th2 


ldab 

radius +#RCHAR- #$03 


jsr 

AS C 2 HEX 


ldaa 

mul 

#RDPX 


addd 

th2 


std 

th2 


ldab 

radius +#RCHAR-#$04 


j«r 

ASC2HEX 


ldaa 

pshb 

psha 

puly 

#$00 

mk.th.2c 

cpy 

#$0000 


beq 

mkth2d 


ldx 

#10 

mkth2f 

ldd 

#RDPI 


addd 

th2 


std 

dex 

th2 


bne 

dey 

xnkth2f 


bra 

mkth2c 

mkth2d 

nop 



ldab 

radius 


cxnpb 

#’-* 


beq 

mkth2e 


ldd 

#$0000 


subd 

th2 


std 

th2 


* Convert theta string to thl (hex) 

* 1 1 s place 


* 10 ’ s place 


* 100* s place 


* Absolute value ok 

* If «-■ leave positive 

* If "+' make negative 

* Since motor sign opposite of 

* coordinate system sign 


* Convert radius string to th2 (hex) 

* If tenths place >* 5 then 

* use 1/2 in. else truncate. 


* 1 1 s place 

* Degrees per one inch 


* 10 1 s place 


* Degrees per one inch 


* If leave positive 

* If "+' make negative 

* Since motor sign opposite of 

* coordinate system sign 
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* Add count/coordinate system offset 


mkth2e 

ldd 

#RZERO 


addd 

th2 


std 

th2 


ldaa 

#'l* 


staa 

fcmd2 

maketh3 

Idab 

height+#RCHAR-#$01 


cmpb 

# 1 5 ' 


bit 

mkth3a 


ldd 

#ZDPI 


ldx 

idiv 

pshx 

pula 

pulb 

#$0002 


bra 

mkth3b 

mkth3a 

ldd 

#$0000 

mkth3b 

std 

th3 


ldab 

height +#RCHAR-#$ 03 


j»r 

AS C 2 HEX 


ldaa 

mul 

#ZDPI 


addd 

th3 


std 

th3 


ldab 

height+#RCHAR-#$04 


jar 

AS C 2 HEX 


ldaa 

pshb 

psha 

puly 

#$00 

mkth3c 

cpy 

#$0000 


beq 

mkth3d 


ldx 

#10 

mkth3f 

ldd 

#ZDPI 


addd 

th3 


std 

dex 

th3 


bne 

dey 

mkth3f 


bra 

mkth3c 

mkth3d 

nop 



ldab 

height 


cmpb 

#' + ’ 


beq 

mkth3e 


cmpb 

#’ * 


beq 

mkth3e 


ldd 

#$0000 


subd 

th3 


std 

th3 

mkth3e 

ldd 

#ZZERO 


addd 

th3 


std 

th3 


ldaa 

#'l' 


staa 

fcmd3 


* Set command flag 


* Convert height string to th3 (hex) 

* If tenths place >■ 5 then 

* use 1/2 in. else truncate. 


* l's place 

* Degrees per one inch 


* 10 1 s place 


* Degrees per one inch 


* If make negative 

* otherwise make positive 

* Since motor sign is the same as 

* otherwise make positive 

* Since motor sign is the same as 

* coordinate system sign 


* Add count /coordinate system offset 


* Set command flag 


zmakcmd nop 
rts 


* outcmdl — Sends motor 1 command out sci port. 


outcmdl ldaa #241 

jsr putsci 

ldaa thl 

jsr acib2h 

ldaa thl+#$01 
jsr scib2h 

ldaa # 1 0 ' 
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staa 

rts 


fcmdl 


* outcmd2 — Sands motor 2 command out sci port. 


outcmd2 Idaa #242 

jsr put sc i 

Idaa th2 

jar scib2h 

Idaa th2+#$01 
jsr scib2h 

Idaa # 1 0 1 

staa £cmd2 

rts 


* out cmd 3 — Sends motor 3 command out sci port. 


outcmd3 Idaa #243 

jsr putsci 

Idaa th3 

jsr scib2h 

Idaa th3+#$01 
jsr scib2h 

Idaa # ' 0 • 

staa f cxnd3 

rts 


* pattbig -- Define centers of overall search pattern. These values 

* will be used to start 

* searches if the first grid search is not successful. 


* 

cntr2 : 

r - r5 

+ 

3*del_r 



* 


theta2 

a 

thetaS 



* 

cntr3 : 

r » r5 

+ 

3*del_r 



* 


theta3 

at 

thetaS + 

3*del_ 

.theta 

* 

cntr4 : 

r - r5 





* 


theta3 

a 

theta5 + 

3*del_ 

.theta 

* 

cntr5 : 

r = r5 

- 

3*del_r 



* 


theta3 

a 

thetaS + 

3*del_ 

.theta 

* 

cntr6 : 

r ■ r5 

- 

3 *del_r 



* 


theta3 

a 

thetaS 



* 

cntr7 : 

r =* rS 

- 

3*del_r 



* 


theta3 

a 

thetaS - 

3*del. 

_theta 

♦ 

cntr8 s 

in 

U 

* 

u 





★ 


theta3 

a 

thetaS - 

3*del. 

_theta 

* 

cntr9 : 

r *» rS 

+ 

3*del_r 



★ 


theta3 

a 

thetaS - 

3*del_ 

.theta 


pattbig psha 
pshb 
pshx 
pshy 

* Determine the number of whole & half inches in 3*del_r. 


ldd 

del_r 

addd 

del_r 

addd 

del_r 

ldx 

#2 

idiv 

std 

half_in 

pshx 

pula 

pulb 

std 

whole_in 


* Modify r values of cntr3, cntr4, and cntr5. 
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ldaa #$00 * Road string. Convert to hex. 

ldab cntrl+#15 

subb #$30 

std tmpl6 

ldab cntrl+#14 

subb #$30 

ldaa #10 

nrul 

addd tap 16 

std tznpl6 

addd whole_in * Add whole_in to hex version of rr string 

ldx #10 * Convert new hex value to rr ASCII string 

idiv 

addd #$30 

stab cntr5+#15 

stab cntr4+#15 

stab cntr3+#15 

pshx 

pula 

pulb 

addd #$30 

stab cntr5+#14 

stab cntr4+#14 

stab cntr3+#14 

Idd hal£_in * if <half_in-»l) then rr.r » rr.r+.5 

cpd #1 

bne nohafsl 

ldaa cntrl+#17 

cznpa # ' 5 * 

beq hafsinl 

ldaa # • 5 * 

staa cntr5+#17 

staa cntr4+#17 

staa cntr3+#17 

bra nohafsl 

hafsinl ldaa #'0* 

staa cntr5+#17 

staa cntr4+#17 

staa cntr3+#17 

ldaa cntrl+#15 

adda #1 

crop a # ' 9 * 

bgt carrysl 

staa cntr5+#15 

staa cntr4+#15 

staa cntr3+#15 

bra nohafsl 

carrysl ldaa #'0* 

staa cntr5+#15 

staa cntr4+#15 

staa cntr3+#15 

ldaa cntrl+#14 

adda #1 

staa cntr5+#14 

staa cntr4+#14 

staa cntr3+#14 

nohafsl nop 

* Modify r values of cntr7, cntr8, and cntr9 

ldd tap 16 * Load hex version of rr string 

subd whole_in * Subtract whole^in from hex version of rr 

* Convert new hex value to rr ASCII string 


ldx 

idiv 

addd 


#$30 
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if <half_in»«l) then rr.r 


rr . r + . 5 


stab 

cntr7+#15 

stab 

cntr8+#15 

stab 

cntr9+#15 

pshx 

pula 

pulb 

addd 

#$30 

stab 

cntr7+#14 

stab 

cntr8+#14 

stab 

cntr9+#14 

ldd 

half_in 

cpd 

#1 

bne 

nohaf a 2 

Idaa 

cntrl+#17 

cmpa 

#'0' 

beq 

haf sin2 

ldaa 

#■0' 

staa 

cntr7+#17 

staa 

cntr8+#17 

staa 

cntr9+#17 

bra 

nohaf s 2 

haf sin2 ldaa 

#'5' 

staa 

cntr7+#17 

staa 

cntr8+#17 

staa 

cntr9+#17 

ldaa 

cntrl+#15 

suba 

#1 

cmpa 

#•0’ 

bit 

carrys2 

staa 

cntr7+#15 

staa 

cntrB+#15 

staa 

cntr9+#15 

bra 

nohaf s2 

carry s 2 ldaa 

#•9' 

staa 

cntr7+#15 

staa 

cntr8+#15 

staa 

cntr9+#15 

ldaa 

cntrl+#14 

suba 

#1 

staa 

cntr7+#14 

staa 

cntr8+#14 

staa 

cntr9+#14 

noha£s2 nop 

* Modify theta 

values of cntr5 

ldaa 

#$00 

ldab 

cntrl+#ll 

subb 

#$30 

std 

turpi 6 

ldab 

cntrl+#10 

subb 

#$30 

ldaa 

#10 

xnul 

addd 

tmpl6 

std 

txnpl6 

ldab 

cntrl+#9 

subb 

#$30 

ldaa 

#100 

mul 

addd 

tmplS 

std 

turpi 6 

subd 

del_theta 

* 


aubd del theta 

aubd del_theta 


cntr6 1 and cntr7 
* Read string. 


Convert to hex. 


* Subtract 3*del_theta from hex version of 

* theta string. Same as adding and including 

* sign on theta. Sign ignored since always 

* negative. 
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#100 


* Convert hex value to theta ASCII string 


ldx 

idiv 

psha 

pshb 

pshx 

pula 

pulb 

addd 

stab 

stab 

stab 

pulb 

pula 

ldx 

idiv 


#$30 

cntr5+#9 

cntr6+#9 

cntr7+#9 


#10 


psha 

pshb 

pshx 

pula 

pulb 


addd 

#$30 

stab 

cntr5+#10 

stab 

cntr6+#10 

stab 

cntr7+#10 

pulb 


pula 


addd 

#$30 

stab 

cntr5+#ll 

stab 

cntr6+#ll 

stab 

cntr7+#ll 

Modify theta 

values of cntr3 

ldd 

trap 16 

addd 

del_theta 


addd 

del_theta 

addd 

del_theta 

ldx 

#100 

idiv 

psha 

pshb 

pshx 

pula 

pulb 

addd 

#$30 

stab 

cntr3+#9 

stab 

cntr2+#9 

stab 

cntr9+#9 

pulb 

pula 

ldx 

#10 

idiv 

psha 

pshb 

pshx 

pula 

pulb 

addd 

#$30 

stab 

cntr3+#10 

stab 

cntr2+#10 

stab 

cntr9+#10 

pulb 

pula 

addd 

#$30 


cntr2 , and cntr9 

* Load hex version of rr string 

* Add 3*del_theta from hex version of 

* theta string. Same as subtracting and 

* including sign on theta. Sign ignored 

* since always negative. 


* Convert hex value to theta ASCII string 
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* Write out the calculated grid centers. 


stab 

cntr3+#ll 

stab 

cntr2+#ll 

stab 

cntr9+#ll 

J«r 

wrcntr 

puly 

pulx 

pulb 


pula 

rts 



pattern — Modify box strings to spread pattern. Looking down from the 
boom and towards boom end , search pattern looks like 


* boxl 

* box4 

* box7 

* Top rows 

* Bottom row: 

* Left Column: 

* Right Column: 


box2 

box3 



boxS 

box6 



box 8 

box9 



r - r5 

+ del_r 



r - r5 

- del_r 



theta ■ 

theta5 

+ del. 

_theta 

theta a 

theta5 

- del. 

.theta 


pattern psha 
pshb 
pshx 
pshy 

* Determine the number of whole & half inches that r must be modified 
ldd del_r 

Idx #2 

idiv 

std half_in 

pshx 

pula 

pulb 

std whole_in 


* Modify r values of boxl, box2, and box3. 

Read string. Convert to hex. 


ldaa 

#$00 * 

ldab 

box5+#15 

subb 

#$30 

std 

trap 16 

ldab 

box5+#14 

subb 

#$30 

Idea 

#10 

mul 


addd 

tmpl6 

std 

tmpl6 

addd 

whole_in * 

Idx 

#10 

idiv 


addd 

#$30 

stab 

boxl+#15 

stab 

boxl +#15 

stab 

box3+#15 

pshx 


pula 


pulb 


addd 

#$30 

stab 

boxl+#14 

stab 

boxl +#14 

stab 

box3+#14 

ldd 

half_in * 


* Add whole_in to hex version of rr string 

* Convert new hex value to rr ASCII string 


if (half_in*«l) then rr.r - rr.r+.5 
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cpd 

#1 


bn© 

nohalf 1 


ldaa 

box5+#17 


cmpa 

# ' 5 * 


beq 

half ini 


ldaa 

#•5' 


■taa 

boxl+#17 


ataa 

box2+#17 


ataa 

box3+#17 


bra 

nohalf 1 


half ini ldaa 

#•0* 


■taa 

boxl+#17 


ataa 

box2+#17 


■taa 

box3+#17 


ldaa 

box5+#15 


adda 

#1 


cmpa 

# ' 9 * 


bgt 

carryl 


■taa 

boxl+#15 


■taa 

box2+#15 


■taa 

box3+#15 


bra 

nohalf 1 


carryl ldaa 

#'0' 


■taa 

boxl+#15 


■taa 

box2+#15 


■taa 

box3+#15 


ldaa 

box5+#14 


adda 

#1 


■taa 

boxl+#14 


■taa 

box2+#14 


■taa 

nohalf 1 nop 

box3+#14 


* Modify r values of box7. 

box 8, and box9 

ldd 

tmp!6 

* Load hex version of rr string 

■ubd 

whola_in 

* Subtract whole_in from hex version of rr 

ldx 

idiv 

#10 

* Convert new hex value to rr ASCII string 

addd 

#$30 


■tab 

box7+#15 


stab 

box8+#15 


stab 

pahx 

pula 

pulb 

box9+#15 


addd 

#$30 


stab 

box7+#14 


■tad? 

box8+#14 


■tad? 

box9+#14 


ldd 

half _in 

* if (half_in»»l) then rr.r * rr.r+.5 

cpd 

#1 


bna 

nohalf2 


ldaa 

box5+#17 


cmpa 

#'0' 


baq 

half in2 


ldaa 

#'0* 


■taa 

box7+#17 


■taa 

box6+#17 


■taa 

box9+#17 


bra 

nohalf 2 


halfin2 ldaa 

# ’ 5 ‘ 


■taa 

box7+#17 


■taa 

box8+#17 


■taa 

box9+#17 


ldaa 

box5+#15 


suba 

#1 
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cnxpa 

bit 

staa 

staa 

staa 

bra 

carry2 ldaa 
staa 
staa 
staa 
ldaa 
suba 
staa 
staa 
staa 

nohalf 2 nop 


#• 0 * 

carry2 

box7+#15 

box8+#15 

box9+#15 

nohalf 2 

#•9* 

box7+#15 

box8+#15 

box9+#15 

box5+#14 

#1 

box7+#14 

box8+#14 

box9+#14 


* Modify theta values of boxl, box4, and box7 
ldaa #$00 * Read atring. 

ldab box5+#ll 

subb *$30 

std tsxpl6 

ldab box5+#10 

subb *$30 

ldaa *10 

mul 

addd tapis 

std taplS 

ldab box5+#9 

subb #$30 

ldaa #100 

mul 

addd taplS 

std taplS 


Convert to hex. 


subd 


del_theta 


Idx 

#100 

idiv 

psha 

pshb 

pshx 

pula 

pulb 

addd 

#$30 

stab 

boxl+#9 

stab 

box4+#9 

stab 

box7+#9 

pulb 

pula 

ldx 

#10 

idiv 

psha 

pshb 

pshx 

pula 

pulb 

addd 

#$30 

stab 

boxl+#10 

stab 

box4+#10 

stab 

box7+#10 

pulb 

pula 

addd 

#$30 

stab 

boxl+#ll 

stab 

box4+#ll 


Subtract del_theta from hex version of 
theta string. Same as adding and including 
sign on theta. Sign ignored since always 
negative. 

Convert hex value to theta ASCII string 
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stab 


box7+#ll 


* Modify theta values of box3 f box 6, and box9 

ldd tinplfi * Load hex version of rr string 



addd 

del theta * Add del_theta from hex version of 

* 


* theta string. Same as subtracting and 

* 


* including sign on theta. Sign ignored 

* 


* since always negative 


ldx 

idiv 

psha 

pshb 

pshx 

pula 

pulb 

#100 * Convert hex value to theta ASCII string 


addd 

*$30 


stab 

box3+#9 


stab 

box6+#9 


stab 

pulb 

pula 

box9+#9 


ldx 

idiv 

psha 

pshb 

#10 


pshx 

pula 

pulb 



addd 

#$30 


stab 

box3+#10 


stab 

box 6 +#10 


stab 

pulb 

pula 

box9+#10 


addd 

#$30 


stab 

box3+#ll 


stab 

box 6 +#11 


stab 

box9+#ll 


j»r 

puly 

pulx 

pulb 

pula 

rts 

vrbox * Print search box coordinates to screen 

* putaway -- Move claw to drop location and open. 

droppt 

fee 

• +00 , +00, -000, +10.5,-03.5' 

put away 

nop 

take claw to top before approaching drop point 

*1 new 

stuff to 

* 

ldx 

#R_DATA 

Mebl 

ldaa 

0,X 

* 

inx 


* 

cpx 

#R_DATA+#MAXIN 

* 

bit 

debl 

* 

* 

ldaa 

# i 1 « * Take claw to top 

* 

staa 

fcmdl 

★ 

staa 

f cxad2 

* 

staa 

fcmd3 

* 

staa 

femde 

* 

ldaa 

#'l* 

* 

staa 

clawcxnd 

* 

ldx 

#R _DATA+ #MAX IN- # ZCHAR * Not* +09.0 ie out of range 



* Ida a #’ ' * and forces the claw all the way up. 

* staa 0 , X 

* inx 

* Idaa # 4 0 4 

* staa 0, X 

* inx 

* ldaa # * 9 * 

* staa 0,X 

* inx 

* ldaa # ■ . 4 

* staa 0,X 

* inx 

* ldaa # 4 0 4 

* staa 0,X 

* jsr checkin 

* jar makecmd 

* jsr docmds2 

* ! end new stuff 

ldy #R_DATA * Go to drop point 

ldx idroppt 

ldaa # 1 1 4 

staa f cjndl 

staa fcmd2 

staa fcmd3 

staa f erode 

ldaa # 4 0* 

staa clawcmd 

ptaway2 ldaa 0 , X 

staa 0,Y 

inx 
iny 

Cpy #R_DATA+#MAXIN 

bit ptaway2 

jsr checkin 

j sr makecmd 

jsr docxnds 

* jsr doout 

rts 


* PRNTDAT — OUTPUT RELEVANT DATA TO SCREEN 


PRNTDAT psha 
pshb 
pshx 
pshy 

* ldaa 

* jsr 
ldx 
ldy 

prthl ldaa 
j*r 
iny 
cpy 
bne 

* ldaa 

* j*r 
ldaa 

jsr 

prthlz nop 

*P cpy 

bis 


# 4 0 4 
putacia 
#REGBAS 
#sci0_in 
0, Y 

putacia 

#sciO_in+#MAXSCI-#2 
prthlz 
# 4 h 4 
putacia 
#SPACE 
putacia 

#sci0_in+#MAXSCI-#$01 * mode info removed for plot 

prthl 


* added for plot 

* added for plot 


ldaa 


#SPACE 

putacia 

putacia 
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* added for plot 

* added for plot 


* 

Idaa 

#•0* 

* 

Ur 

putacia 


ldy 

#acil_in 

prth.2 

Idea 

0,Y 


j*r 

putacia 


iny 



cpy 

#acil_in+#MAXSCI-#2 


bne 

prth2z 

• 

ldaa 

# ' h 1 

* 

jar 

putacia 


ldaa 

#SPACE 


j«r 

putacia 

prth2z 

nop 


*P 

cpy 

#Bcil_in+#MAXSCI- ; 


bla 

prth2 


ldaa 

# SPACE 



putacia 


Ur 

putacia 

* 

ldaa 

#'0' 

* 

Ur 

putacia 


ldy 

#«ci2_in 

prth3 

ldaa 

0,Y 


Ur 

putacia 


iny 



cpy 

#*ci2_in+#MAXSCI-#2 


bne 

prth3z 

* 

ldaa 

#'h* 

* 

Ur 

putacia 


ldaa 

*SPACE 


Ur 

putacia 

prth3z 

nop 


*P 

cpy 

#aci2_in+#MAXSCI- ; 


bla 

prth3 


ldaa 

#SPACE 


jar 

putacia 


jar 

putacia 

prclw 

ldab 

PORTA, X 


bmi 

itson 

itsoff 

ldaa 

#'0' 


Ur 

putacia 

* 

ldaa 

#'0' 

* 

Ur 

putacia 

* 

ldaa 

#'p' 

* 

Ur 

putacia 

* 

ldaa 

# ' e * 

* 

Ur 

putacia 

* 

ldaa 

#’n' 

* 

Ur 

putacia 

* 

ldaa 

#SPACE 

* 

Ur 

putacia 

* 

Ur 

putacia 


bra 

ZPRNTDT 

itson 

ldaa 

# * 1 1 


Ur 

putacia 

* 

ldaa 

#'C» 

* 

Ur 

putacia 

* 

ldaa 

#•1' 

* 

Ur 

putacia 

* 

ldaa 

#'o' 

* 

Ur 

putacia 

* 

ldaa 


* 

Ur 

putacia 

* 

ldaa 

# *e 1 

* 

Ur 

putacia 

* 

ldaa 

#'d' 


* added for plot 


* mode info removed for plot 


added for plot 
added for plot 


* added for plot 


* mode info removed for plot 
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jar putacia 


ZPRNTDT ldaa 
jar 


♦CR 

putacia 


puly 

pulx 

pulb 

pula 

rta 


* showmap — print depth map to screen (3x3 grid) 


shoumap paha 
pshb 
pahx 
pahy 

ldaa 

hgtl 

jar 

b ini hex 

ldaa 

hgtl+1 

jar 

bin! hex 

ldaa 

♦SPACE 

jar 

putacia 

ldaa 

hgtl 

Ur 

binlhex 

ldaa 

hgt2+l 

jar 

bin2hex 

ldaa 

♦SPACE 

Ur 

putacia 

ldaa 

hgt3 

Ur 

binlhex 

ldaa 

hgt3+l 

Ur 

binlhex 

ldaa 

♦CR 

jar 

putacia 

ldaa 

hgt4 

Ur 

bin2hex 

ldaa 

hgt4+l 

jar 

binlhex 

ldaa 

♦SPACE 

Ur 

putacia 

ldaa 

hgt5 

Ur 

binlhex 

ldaa 

hgt5+l 

j ®r 

binlhex 

ldaa 

♦SPACE 

Ur 

putacia 

ldaa 

hgt6 

Ur 

bin2hex 

ldaa 

hgt6+l 

jar 

bin2hex 

ldaa 

♦CR 

jar 

putacia 

ldaa 

hgt7 

Ur 

binlhex 

ldaa 

hgt7+l 

jar 

binlhex 

ldaa 

♦SPACE 

jar 

putacia 

ldaa 

hgt8 

jar 

binlhex 

ldaa 

hgt8+l 

jar 

bin2hex 

ldaa 

♦SPACE 

jar 

putacia 

ldaa 

hgt9 

jar 

binlhex 
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ldaa hgt9+l 

j ar bin2hex 

ldaa #CR 

jar putacla 

ldaa target 

jar putacia 

ldaa #CR 

jar putacia 

puly 

pulx 

pulb 

pula 

rta 


* ayntax -- print input command ayntax to acreen 


ayntax nop 

ldaa #'fi' 

jar putacia 

ldaa #'p* 

jar putacia 

ldaa #*p’ 

jar putacia 

ldaa # 1 , • 

jar putacia 

ldaa #'fi' 

jar putacia 

ldaa # 'q* 

jar putacia 

ldaa #'q' 

jar putacia 

ldaa # ' , 1 

jar putacia 

ldaa #'&* 

jar putacia 

ldaa # ' t • 

jar putacia 

ldaa # ' t ' 

jar putacia 

ldaa # • t • 

jar putacia 

ldaa # ' , 1 

jar putacia 

ldaa #’fi’ 

jar putacia 

ldaa # ’ r ’ 

jar putacia 

ldaa # ' r ' 

jar putacia 

ldaa # • . • 

jar putacia 

ldaa # ' r 1 

jar putacia 

ldaa # 1 , 1 

jar putacia 

ldaa # ' fi ' 

jar putacia 

ldaa # 1 z 1 

jar putacia 

ldaa # ' z ' 

jar putacia 

ldaa #•.’ 

jar putacia 

ldaa # • z ■ 

jar putacia 

ldaa #CR 

jar putacia 
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rts 


* th3&2th3s() - Converts 4 ascii bytes in th3a to 2 hex bytes in th3s. 


th3a2th3s nop 


psha 


ldaa 

th3a 

jar 

HKXBIN 

ldaa 

th3a+#l 

jar 

HEXBIN 

ldaa 

SHFTREG+#1 

staa 

th3s 

ldaa 

th3a+#2 

jar 

HEXBIN 

ldaa 

th3a+#3 

jar 

HEXBIN 

ldaa 

SHFTREG+#1 

staa 

th3s+#l 

pula 


rts 



* th32hgts() - Converts 2 hex bytes in th3 to 4 ascii 

* bytes in hgts (hhh.h) . 


th32hgts nop 
rts 


* vrrbox -- print command lines for nine search points. 


wrbox 


wrboxl 


wrbox2 


wrbox3 


wrbox4 


psha 


pshb 


pshx 


pshy 


ldy 

#boxl 

ldaa 

0,Y 

jsr 

putacia 

iny 


cpy 

#box 1 + #MAXIN 

bit 

wrboxl 

ldaa 

#CR 

j«r 

putacia 

ldy 

#box2 

ldaa 

0,Y 

j»r 

putacia 

iny 


cpy 

#box2+#MAXIN 

bit 

wrbox2 

ldaa 

#CR 

jar 

putacia 

ldy 

#box3 

ldaa 

0,Y 

j»r 

putacia 

iny 


cpy 

#box3+#MAXIN 

bit 

wrbox3 

ldaa 

#CR 

jar 

putacia 

ldy 

#box4 

ldaa 

0 # Y 

jar 

putacia 

iny 


cpy 

#box4+#MAXIN 
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wrboxS 


wrboxS 


wrbox7 


wrbox8 


wrbox9 


bit 

wrbox4 

ldaa 

#CR 

jar 

putacia 

ldy 

#box5 

ldaa 

0,Y 

jar 

iny 

putacia 

cpy 

#box5+#MAXIN 

bit 

wrbox5 

ldaa 

#CR 

jar 

putacia 

ldy 

#box6 

ldaa 

0# Y 

jar 

iny 

putacia 

cpy 

#box6+#MAXIN 

bit 

wrbox6 

ldaa 

#CR 

jar 

putac ia 

ldy 

#box7 

ldaa 

Or Y 

jar 

iny 

putacia 

cpy 

#box7+#MAXIN 

bit 

wrbox7 

ldaa 

#CR 

jar 

putacia 

ldy 

#box8 

ldaa 

0# Y 

jar 

iny 

putacia 

cpy 

#box8+#MAXIN 

bit 

wrbox8 

ldaa 

#CR 

jar 

putacia 

ldy 

#box9 

ldaa 

0,Y 

jar 

iny 

putacia 

cpy 

#box9+#MAXIN 

bit 

wrbox9 

ldaa 

#CR 

jar 

puly 

pulx 

pulb 

pula 

rts 

putacia 


* wrcntr — print command lines for nine search grid centers. 


wrcntr psba 
pshb 
pshx 
pshy 

ldy 

wrcntr 1 Idea 
jar 
iny 
cpy 


#cntrl 

0,Y 

putacia 

#cntrl+#MAXHI 
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bit 

ldaa 

jsr 

ldy 

wrcntr2 ldaa 
j«r 
iny 
cpy 
bit 
ldaa 
j*r 

ldy 

wrcntr3 ldaa 
jar 
iny 
cpy 
bit 
ldaa 
jar 

ldy 

wrcntr4 ldaa 
jar 
iny 
cpy 
bit 
ldaa 
jar 

ldy 

wrcntrS ldaa 
jar 
iny 
cpy 
bit 
ldaa 
jar 

ldy 

wrcntr6 ldaa 
jar 
iny 
cpy 
bit 
ldaa 
jar 

ldy 

wrcntr7 ldaa 
jar 
iny 
cpy 
bit 
ldaa 
jar 

ldy 

wrcntrS ldaa 
jar 
iny 
cpy 
bit 
ldaa 
jar 

ldy 

wrcntr9 ldaa 


wrcntrl 

#CR 

putacia 

#cntr2 

0,Y 

putacia 

#cntr2+#MAXIN 

wrcntr2 

#CR 

putacia 

#cntr3 

0,Y 

putacia 

#cntr3+#MAXIN 

wrcntr3 

#CR 

putacia 

tent r 4 
Or Y 

putacia 

#cntr4+#MAXIN 

wrcntr4 

#CR 

putacia 

#cntr5 

Or Y 

putacia 

#cntr5+#MAXIN 

wrentrS 

#CR 

putacia 

#cntr6 
0, Y 

putacia 

#cntr6+#MAXIN 

wrcntr6 

#CR 

putacia 

#cntr7 
Or Y 

putacia 

#cntr7+#MAXIN 

wrcntr7 

#CR 

putacia 

#cntr8 

0 r Y 

putacia 

#cnt r 8 +#MAXIN 

wrcntr8 

#CR 

putacia 

#cntr9 

OrY 



jsr 

put&cla 

iny 


cpy 

#cntr9+#MAXIN 

bit 

wrcntr9 

ldaa 

#CR 

jsr 

putacia 

puly 


pulx 


pulb 


pula 


rts 



* wrerrO -- ■ 

syntax error" 

wrarrO nop 


Idaa 

#'W 

jsr 

putacia 

ldaa 

#*R' 

jsr 

putacia 

ldaa 


jsr 

putacia 

ldaa 

#'R' 

jsr 

putacia 

ldaa 

# 'R ' 

jsr 

putacia 

ldaa 

#‘0' 

jsr 

putacia 

ldaa 

#': • 

jsr 

putacia 

ldaa 

#SPACE 

jsr 

putacia 

ldaa 

#SPACE 

jsr 

putacia 

ldaa 

#'S' 

jsr 

putacia 

ldaa 

#'Y' 

jsr 

putacia 

ldaa 

#'n* 

jsr 

putacia 

ldaa 

#'t* 

jsr 

putacia 

ldaa 

# ' a ’ 

jsr 

putacia 

ldaa 

#‘x’ 

jsr 

putacia 

ldaa 

#SPXCE 

jsr 

putacia 

ldaa 

# ' e * 

jsr 

putacia 

ldaa 

# ' r ' 

jsr 

putacia 

ldaa 

# ' r ' 

jsr 

putacia 

ldaa 

#’o* 

jsr 

putacia 

ldaa 

# ' r ' 

jsr 

putacia 

ldaa 

#CR 

jsr 

putacia 

rts 


* 

* THE INTERRUPT SERVICE ROUTINES 

* 
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* ACXA_ISR -- Interrupt service routine occur* when ACIA input received 


ACIA_ISR nop 



jsr 

inacia 


jar 

checkin 

* 

jar 

makecmd 


ldaa 

detain 


cmpa 

#* 1* 


bne 

zacia 


jar 

PRNTDAT * print 

zacia 

rti 


* SCI0_ 

#** 

ISR — 
*** 

Interrupt service routine 

SCI0_ISR nop 


* 

ldaa 

# ' A* 

* 

jar 

putacia 


ldx 

#REGBAS 


bclr 

PORTA, X #9601110000 


j«r 

getsci 

* 

jar 

putacia 


ldy 

#$FFFF 

getsciO 

dey 



beg 

ZSCI0 


ldab 

SCSR,X 


bitb 

#$20 


beg 

getsciO 


ldaa 

SCDR , X 

* 

jar 

putacia 


cmpa 

#$F0 


bne 

ZSCI0 

GETZM0 

ldy 

#sci0_in 

SciO 

jar 

getsci 


cmpa 

#$F0 


beg 

GETEM0 


staa 

0,Y 

* 

jar 

iny 

putacia 


cpy 

#*ci0_in+#MAXSCI-#$01 


bis 

sciO 

* 

ldaa 

♦SPACE 

* 

jar 

putacia 

* 

ldaa 

#•0' 

* 

jar 

putacia 

* 

ldaa 

#CR 

* 

jar 

putacia 


ldaa 

#'1' 


staa 

ffbkl 

ZSCI0 

ldaa 

#*00000001 


staa 

TFLG1, X 


ldaa 

PORTA, X 


ora 

#9601110000 


staa 

rti 

PORTA, X 


home position before maneuver. 


occur* when SCIO input received. 


* read trigger character & discard 

* Y * $FFFF 

* Y » Y - 1 

* if Y«0 then return 

* if (character not received) then 

* goto getsciO 

* read received character 

* if (char ! * FO) then return 

* read and store MAXSCI characters 

* echo characters, space, 0, CR 


* Clear IC3 bit (PAO) 

* PA6,PA5,PA4 

* P0 : 000, Pi: 001, P2 : 010, P3 : Oil 

* P4 : 100, P5 : 101, P6 : 110, P7 : 111 


* SCI1_ISR -- Interrupt service routine occurs when SCI1 input received. 


SCI1_ISR nop 

* ldaa 

* jsr 
idx 
bclr 
jsr 
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# ' B ' 
putacia 
#REGBAS 

PORTA, X #9601100000 
getsci 


read trigger character & discard 


c-a. 



* 

jsr 

putacia 



ldy 

*$F7F7 

* 

getacil 

dey 


* 


beg 

ZSCIl 

* 


ldab 

SC SR, X 



bitb 

#$20 

* 


beq 

getscil 

* 


ldaa 

SCDR , X 

* 

* 

jsr 

putacia 



czopa 

#$F0 

* 


bne 

ZSCI1 


GETEM1 

ldy 

#acil_in 

* 

scil 

jsr 

getsci 

* 


cmpa 

#$F0 



beq 

GETEM1 



staa 

0 , Y 


* 

jsr 

putacia 



iny 




cpy 

#«cil_in+#MAXSCI-#$01 



bis 

scil 


* 

ldaa 

#SPACE 


* 

jsr 

putacia 


* 

ldaa 

#' 1* 


* 

jsr 

putacia 


* 

ldaa 

#CR 


# 

jsr 

putacia 



ldaa 

# ' 1 * 



staa 

ffbk2 


ZSCI1 

ldaa 

#*00000010 



staa 

TFLG1,X 

* 


ldaa 

PORTA, X 

* 


ora 

#*01110000 

* 


staa 

PORTA, X 

* 


rti 



* SCI2_ 

-*** 

ISR — 
* * * 

Interrupt service routine 

oc 

SCX2_ISR nop 



* 

ldaa 

#’C’ 


* 

jsr 

putacia 



ldx 

#REGBAS 



bclr 

PORTA, X #*01010000 



jsr 

getsci 

* 

* 

jsr 

putacia 



ldy 

#$FFFF 

* 

getsci2 

dey 


* 


beq 

ZSCI2 

* 


ldab 

SCSR,X 



bitb 

#$20 

* 


beq 

getsci2 

* 


ldaa 

SCDR, X 

* 

* 

jsr 

putacia 



cmpa 

#$F0 

* 


bne 

ZSCI2 



Y - 1 


if (character not received) then 
goto getacil 


echo characters, space, 0, CR 


Clear IC2 bit (PA1) 

PA6 , PA5 , PA4 

P0:000,Pl:001,P2:010,P3:011 

P4 : 100, P5 : 101, P6 : 110, P7 : 111 


GETEM2 ldy #sci2_in 

sci2 jsr getsci 

cxnpa #$70 

bttQ GETEM2 

st&a 0, Y 

* jsr putacia 

iny 

cpy #sci2_in+#MAXSCI-#$01 


read trigger character & discard 

Y * $FFFF 

Y ■ Y - 1 

if Y*0 then return 


goto getsci2 
read received character 
if (char !- F0) then return 

* read and store MAXSCI characters 

* echo characters, space, 0, CR 
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bis 

sci2 


* 

ldaa 

#SPACE 


* 

dar 

putacia 


* 

ldaa 

#•2* 


* 

dar 

putacia 


* 

ldaa 

#CR 


* 

dar 

putacia 



ldaa 

#•1' 



staa 

ffbk3 


ZSCI2 

ldaa 

#*00000100 



staa 

TFLG1,X 

* Clear ICl bit (PA2 ) 


ldaa 

PORTA, X 

* PA6 , PAS , PA4 


ora 

#*01110000 

* PO : 000, PI: 001, P2 : 010, P3 : 011 


staa 

PORTA, X 

* P4 : 100, P5 : 101, P6: 110, P7 : 111 


rti 




* SLAK_ISR — Interrupt service routine occur* when PA3 goes low. 


* Continue only if PA3 is low. 


* 

This occurs whenever cable 

SLAK_ISR ldx 

#REGBAS 


brclr 

PORTA, X #*00001000 aslak 

jjnp 

zslak 


aslak bclr 

TMSK1,X #*00001000 

* : 

ldaa 

#243 * write 

to 

dar 

putsci 


ldaa 

#*s* 


jsr 

putsci 


ldaa 

#'T' 


jsr 

putsci 


ldaa 

#’0' 


jar 

putsci 


ldaa 

#'P' 


Jar 

putsci 


* Wait for reply. Update sci2_in. 


ldx 

#REGBAS 


bclr 

PORTA, X #*01010000 


dar 

getsci 

* 

ldy 

#$FFFF 


getslk2 dey 



beq 

ZSLK2 


ldab 

SCSR, X 


bitb 

*$20 

* 

beq 

getslk2 

* 

ldaa 

SCDR , X 

* 

cxnpa 

#$F0 

* 

bne 

ZSLK2 


GETSLK3 ldy 

#«ci2_in 

* 

slk2 dar 

getsci 

* 

C20pa 

#$F0 


beq 

GETSLK3 


staa 

0, Y 


iny 

cpy 

#«ci2_in+#MWCSCI-#$01 


bis 

slk2 


ZSLK2 ldaa 

#*00000100 


staa 

TFLG1, X 

* 

ldaa 

PORTA, X 

* 

ora 

#*01110000 

* 

staa 

PORTA, X 

* 

* above lines 

taken from sci2_isr 



* Disable IC4 interrupt 


read trigger character & discard 

Y » $FFFF 

Y » Y - 1 

if Y»0 then return 

if (character not received) then 
goto getslk2 

read received character 
if (char FO) then return 


read and store MAXSCI characters 
echo characters , space , 0, CR 


Clear IC1 bit (PA2) 
PA6,PA5,PA4 
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j sr doout 

** Convert received position in aci2_in (ASCII) to deaired tfa.3 (ASCII) 

* Strip th3a from sci2_in 

ldaa aci2_in 

staa th3a 

ldaa sci2_in+#l 

staa th3a+#l 

ldaa aci2_in+#2 

ataa th3a+#2 

ldaa aci2_in+#3 

ataa th3a+#3 

* Convert th3a to th3s 

jar th.3a2th.38 

ldd th3s 

std turpi 6 

* Increase height by .5" to 1" to restore tension. 

ldd th3s * Convert th3s to angle from ZZERO 

subd # ZZERO 

std tmpl6 

ldd tmplfi * If angle is negative, take absolute 

ble negth3s * value and set flag (stphgt) to ' - ' 

ldaa # • ' 

staa stphgt 

bra th3cont 

negth3s ldaa # 1 - 1 

ataa stphgt 

ldd #$0000 

subd tmpl6 

std tmpl6 

th3cont nop 

ldx #ZDPI 

ldd tmplfi 

idiv 
pshb 

#0 * Calculate absolute value of angle 

#ZDPI * that corresponds to number of whole 

tmpl6 * inches 

atpl 

pulb 

ldaa #2 

mul 

cpd #ZDPI 

bge anglge 

angllt nop 

ldaa stphgt 

cmpa # 1 - • 

beq case3 

bra caae2 

anglge nop 

ldaa stphgt 

cmpa # * - * 

beq case# 

bra caael 

easel nop tmplfi * tmplfi + 1.5*ZDPI 

ldd tmplfi 

addd #ZDPI 

std tmplfi 

ldd #ZDPI 

ldx #2 


* Recall remainder (R) 

* If 2*R<ZDPI then 

If (angle below zero) then 
goto case3 

else 

goto case2 

End if 

else 

If (angle 6 zero) then 
goto case# 

else 

goto easel 

Endif 

Endif 


ldd 

stpl addd 
std 
dex 
bne 


* Divide angle by ZDPI, to determine 

* ZZERO 


91 




idiv 

pshx 

pula 

pulb 

addd 

turpi 6 


std 

turpi 6 


bra 

Stp2 

case2 

nop 



ldd 

turpi 6 


addd 

#ZDPI 


std 

tmp!6 


bra 

stp2 

case3 

nop 



ldd 

#ZDPI 


ldx 

*2 


idiv 

pshx 

pula 

pulb 

subd 

tmp!6 


std 

tmpl6 


ldd 

#0 


subd 

tmpl 6 


std 

tmpl6 


bra 

stp2 

case4 

nop 



bra 

stp2 

stp2 

Idaa 

stphgt 


cmpa 

#•-* 


beg 

stp3 


ldd 

# ZZERO 


addd 

tmpl6 


std 

th3 


bra 

fltp4 

stp3 

ldd 

# ZZERO 


subd 

tmpl6 


std 

th3 

stp4 

std 

tmpl 6 


ldd 

th3 


addd 

#ZDPI 


std 

th3 


ldaa 

# * 1 ' 


staa 

f stop 


staa 

fcmd3 

zslak 

nop 



ldaa 

#9600001000 


staa 

TFLG1, X 


rti 



tmpl6 m topl6 + 1. 0*ZDPI 


tnrplfi m tnpl6 — 0.5*ZDPI 


tmplfi * tap 16 


* Convert angle from Z coordinate system 

* to absolute motor angle by adding angle 

* to ZZERO if positive and subtracting 

* angle from ZZERO if negative. 


* Add an additional inch to height 

* so that clay can be pinned instead 

* of scooped. 

* set stop flag 

* set new commands flag 

* reset interrupt flag IC4 


* THE MAIN PROGRAM 


START 


zina in 


Ids 

#8tack 

jar 

init 

jar 

go home 

ldaa 

detain 

cmpa 

#•!' 

beg 

znna 

ldaa 

ffbkl 

cmpa 

#'1' 

beg 

sma 

ldaa 

ffbk2 
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znna 

mainl 


main2 


cmpa # * 1 1 

beq mna 

Idaa ffbk3 

cmpa ♦ 1 1 1 

beq mna 

bra zmain 

bra mainl 

ldaa datain 

cmpa # * 1 1 

beq znain2 

jar doout * unaolicited feedback received 

jmp zmain 

nop 

j ar docmda 

jar findit 

j ar put away 

j ar gohame 

jmp zmain 

jm p zmain 
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D. Motor Controller Program Listing 
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MTR9.ASM — Must be linked with MTR1INC . ASM 


PORTA pins: 

0: low bit in 2 bit CPU address 

Read from DIP switch 1. 

Is hi bit in 2 bit CPU address 
Read from DIP switch 1. 

2s Q input from 74LS74. Driven hi 
by index pulse. Driven lo 
by reset CLR (lo-hi on PB7) 

3s Step (5210) 

4: Direction (5210) 

5s Write Input (LS7166) 

6s Read Input (LS7166) 

7s Control /Data Input (LS7166) 

PORTB pins: 

7; lo-hi clears 74LS74 (CLR1) 

7/18/94 — RMB 

Added routine to seek index (home 
position) and reset counter. 

Reads PA2 and steps is not high. 

When high, toggles CLR on 74LS74 

(PB7) . 

10/18/94 — RMB (mtr2 .asm) 

Added SCI Interrupt Service Routine 
to accept incoming command only if 
fa or fl is first character. 

10/19/94 -- RMB (mtr3 .asm) 

Added small, medium, and large step option 
11/15/94 — RMB (mtr4 .asm) 

Repaired home seek on power up. Routine disturbed by s/m/1 option. 
11/16/94 -- RMB (mtr5 .asm) 

Modified input command format to a one byte address 
and four bytes representing 

the ascii version of a hexadecimal number of steps. 

During the SCI_ISR, the command is check to ensure it is valid. 

A flag is set to show that a new command has been received. 

During the main loop, this command is interpreted and sent to 
the motor. Each time a command is executed, a ASCII string is output 
consisting of d (master address, "fO"), six characters representing 
the hex value of the count (512 counts / 360 degrees), and four 
characters representing the issued c omman d. 

11/28/94 -- RMB (mtr6 .asm) 

Make input command and output count in degrees. Appropriate 
conversions must take place in code (400 steps /3 60 degrees and 
360 degrees/ 5 12 counts) . Position output is limited to a two 
byte hex n umb er (represented by four ASCII characters). Maximum 
and minimum values of theta are stored as lim_min and lim_max. 

Theta_d is forced to remain within this range. 

11/29/94 — RMB (mtr7.asm) 

Controller can operate closed loop (using encoder feedback) or 
open loop (using estimated theta) . On power-up, mode is closed 
loop. Mode (o or c) is echoed with theta and c ommand . Cause 
(Is nonchanging count, 2: nonzero at startup, 3: unreasonable 

variation between actual and estimated count) 

12/8/94 — RMB (mtr8.asm) 

Pause after two address output to allow master 
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hardware time to select port 


12/19/94 — RMB (mtr9.asm) 

Modified sci_isr to allow stop command (address, S, ***) . Modified 
STEPIT routine to check for new command before every step. If new 
command is received, routine resets. If new c omman d la STOP, 
theta_d is set to theta. Modified estimated theta (in STEPIT) to 
allow for an interrupt during motion. 


COMSIZE 

equ 

$0004 

R_DATA 

equ 

$0100 

CR 

equ 

$0D 

LF 

equ 

$ 0A 

org 

R_DATA 


rdata 

rmb 

4 

tdata 

nnb 

1 

byte 2 

rmb 

1 

bytel 

rmb 

1 

byteO 

rmb 

1 

theta_d 

rmb 

2 

dxf lag 

rmb 

1 

f stop 

rmb 

1 

theta 

rmb 

2 

delta_th 

rmb 

2 

mode 

rmb 

1 

mode flag 

rmb 

1 

theta_e 

rmb 

2 

theta_o 

rmb 

2 

cnterr 

rmb 

2 

temp 

rmb 

2 

tmpl6 

rmb 

2 

steps 

rmb 

2 

SHFTREG 

rmb 

2 

TMP1 

rmb 

1 

STACKAREA 

rmb 

30 

STACKTOP 

rmb 

1 

ORG 

$b700 


waitcnt 

rmb 

2 

ADDRESS 

rmb 

1 

lim_min 

rmb 

2 

lim_max 

rmb 

2 

MAXERR 

rmb 

2 


ORG $b600 
jmp $d000 
ORG $b604 
jmp SCI_ISR 
ORG waitcnt 

FDB $08FF * * M# mtrl: FFFF 

ORG ADDRESS 

FCB $F3 * M# mtrl: Fl 

ORG lim_min 

FDB $F1F0 * M# mtrl: 0000 

ORG lim__max 

FDB $0000 * M# mtrl: 00E1 

ORG MAXERR 

FDB $0010 

ORG $fffe 

FDB $b600 

ORG $ffd6 

FDB $b604 

* EPROM begins at $D000. 

ORG $D000 


mtr2 : 

08FF 

mtr3 : 

08FF 

mtr2 : 

F2 

mtr3 : 

F3 

mtr2 : 

0000 

mtr3 : 

FIFO 

mtr2 : 

0276 

mtr3 : 

0000 
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jxnp STARTUP 


I NIT ldx #REGBAS 
jar INITA 
jar INITOP 
jar ONSCI 
j ar GOHOME 
jar INITVAR 
cli 


♦INITVAR -- Initialize variablea 


* rdata:rdata+l = 0 


* mode flag * *0' 

* mode ■ 'c' 

* theta_e: theta_e+l ■ 0 

* theta_o: theta_o+l * 0 


♦INITA — MAKE PINS 3 & 7 OF PORTA OUTPUTS 


INITA ldaa #*10001000 
ataa PACTL , X 
rta 

* INITIALIZE THE OPTICAL ENCODER 

* COUNTER CHIP (LS7166) 

INITOP j ar RSTCNTR 

jar SETQR 
jar SETICR 
rta 

* RESET COUNTER TO ZERO ON STARTUP 

RSTCNTR paha 

pahb 

pahx 

pahy 

ldx #REGBAS 

ldaa #*11111111 

ataa DDRC,X 

ldaa #*00000100 

ataa PORTC,X 

jar WR REG 

puly 

pulx 

pulb 

pula 

rta 


♦ TOGGLE WRITE BIT TO WRITE BYTE ON 

* PORTC TO LS7166. 

WRREG paha 

pahb 

pahx 

pahy 

ldx #REGBAS 
ldaa PORTA, X 


INITVAR ldaa #0 

ataa rdata 
ataa rdata+1 
ldaa #’0' 
ataa cbcflag 
ataa fatop 
ataa modeflag 
ldaa #■<=• 
ataa mode 
Idd #0 
atd theta_e 
atd theta_o 
rta 
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ora #*11100000 

ataa PORTA, X 

anda #*11011111 

ftaa PORTA, X 

ora #*11100000 

ataa PORTA, X 

puly 

pulx 

pulb 

pula 

rta 


* SET QUADRATURE REGISTER FOR Xl 

* OPERATION 

SETQR paha 

pahb 

pahx 

pahy 

ldx #REGBAS 

ldaa #*11111111 

ataa DDRC,X 

ldaa #*11111101 

ataa PORTC,X 

jar WRREG 

puly 

pulx 

pulb 

pula 

rta 


* SET INPUT CONTROL RESIGTER 

* ENABLE INPUTS A & B 

SETICR paha 

pahb 

pshx 

pahy 

ldx #REGBAS 

ldaa #*11111111 

ataa DDRC,X 

ldaa #*01001000 

ataa PORTC,X 

jar WRREG 

puly 

pulx 

pulb 

pula 

rta 


* turn SCI on. 9600 baud. 

ONSCI paha 
pahb 
pahx 
pahy 

ldx #REGBAS 
ldaa #BAUD_9600 
ataa BAUD, X 

ldaa #*00001000 * wakeup by addreaa mark (MSB*1) 
ataa SCCR1,X 
ldaa #*00101110 

ataa SCCR3,X * enable SCI transmit 6 receive (wake-up mode) 

puly 

pulx 

pulb 

pula 

rta 
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* STEP MOTOR BACKWARD UNTIL OPTO INDEX 

* GOES HIGH. FORWARD ONE STEP. RESET 

* COUNTER. RESET 74LS74. 

GOHOME pah* 

pahb 

pahx 

pahy 

ldx #RE6BAS 

* * toggle CLR1 (PB7 ) on 74LS74 to aet 

* * Q (PA2) low 

Ida* PORTB, X 
and* #*01111111 
ataa PORTB ,X 
ora #*10000000 
ataa PORTB ,X 

* * check for high on PA2 

* * (Implies that index haa 

* * pulaed) . 

GOHOME 1 Ida* PORTA, X 

anda #*00000100 
bne RESETEM 

* backward one atep aince not yet at index 

jar MFWD1 * M# mtrl: MREV1 mtr2 2 MREV1 mtr3 2 MFWD1 

* pauae before next atep neceaaary ao that motor haa tine to reapond 

jar SLODOWN 
bra GOHOME 1 

RESETEM nop 

* Clear 741a74 (aet Q low) atep FWD (REV) one atep ao that zero cound ia 

* firat atep with low Q. 

jar MREV1 * M# mtrl: MFWD1 mtr2 2 MFWDl mtr3 : MREV1 

jar SLODOWN 

* reaet 74LS74 (Q ia lo) 

ldaa PORTB, X 
anda #*01111111 
ataa PORTB, X 
ora #*10000000 

ataa PORTB, X 

jar RSTCNTR * reaet counter to 0 (LS7166) 

jar RDCNTR * read counter 

Idd bytel * if ( -1 < bytel:byte0 < 1 ) 

cpd #$0001 * goto CLOSED2 

bgt OPEN2 * else 

cpd #$FFFF * goto OPEN2 

bit OPEN2 * endif 

bra CLOSED2 


* mode * 'o' 

* mode flag » '2' 

* goto RESETZ 

CLOSED2 ldaa # , c* 

ataa mode * mode ■ 1 c 1 

ldaa #'0' 

staa mode flag * modaflag » ’O’ 

bra RESETZ * goto RESETZ 

RESETZ puly 
pulx 
pulb 
pula 
rta 

* loop counta down from waitcnt 

* to kill time between atep 

* comsaanda 


OPEN2 ldaa # ' o ' 
ataa mode 
ldaa #'2' 
staa modeflag 
bra RESETZ 
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SLODOWN paha 


pahb 


Idad 

waitcnt 

SLOl subd 

*$0001 

bna 

SLOl 

pulb 


pula 


rta 



* Sand a char out o£ SCI. 
OUTSCI paha 
pahb 
pahx 
pshy 

ldx #REGBAS 
OUTSCI 1 ldaa SCSR,X 



blta *$80 



baq OUTSCI1 

* loop if not 

* 

* raady/ at ill 

* 

ldaa tdata 

* xmittiny . 


ataa SCDR,X 

puly 

pulx 

pulb 

pula 

* aand char 


OUTSCIX rta 


* SET UP THE LS7166 TO READ THE 

* COUNTER REGISTER ON 

* THE LS7166. 

RDCNTR pflha 

pshb 

pahx 

pahy 

ldx #REGBAS 

ldaa #*11111111 

ataa DDRC , X 

ldaa #*00000011 

ataa PORTC,X 

jar WRREG 

ldaa #*00000000 

ataa DDRC , X 

j sr RDDATA 

puly 

pulx 

pulb 

pula 

rta 


* READ THE THREE BYTE COUNTER 

* REGISTER ON THE LS7166 


RDDATA paha 


pshb 


pahx 


pahy 


ldx 

*REGBAS 

ldaa 

PORTA, X 

anda 

#*01111111 

ora 

#*01100000 

ataa 

PORTA, X 

anda 

**00111111 

ataa 

PORTA, X 

ldab 

PORTC , X 



stab byteO 

ora #*01100000 

staa PORTA, X 

anda #*00111111 

staa PORTA, X 

ldab PORTC , X 

stab bytal 

ora #*01100000 

staa PORTA, X 

anda #*00111111 

Staa PORTA, X 

ldab PORTC, X 

stab byte2 

ora #*01100000 

staa PORTA, X 

puly 

pulx 

pulb 

pula 

rts 


* PRCNT — Convert the six byte counter value to a two byte value (degrees) 

* and store at theta: theta+ 1 


PRCNT psha 
pshb 
pshx 
pshy 

ldx #REGBAS 

Idaa bytel 
jsr TO ASCI I 
pshb 

staa tdata 
j sr OUTSCI 
pula 

staa tdata 
j sr OUTSCI 

ldaa byteO 
jsr TOASCII 
pshb 

staa tdata 

jsr OUTSCI 
pula 

staa tdata 

jsr OUTSCI 

PRCNTX puly 
pulx 
pulb 
pula 
rts 


* PRTHETA — Convert the six byte counter value to a two byte valu^ (degrees) 

* and store at theta: theta+1 


PRTHETA psha 
pshb 
pshx 
pshy 

ldx #REGBAS 
jsr DIV2DEG 


ldaa theta 
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jar TOASCII 
pshb 

staa tdata 
j sr ODTSCI 
pula 

staa tdata 

j sr OUTSCI 

ldaa theta+1 
jsr TOASCII 
pshb 

ataa tdata 

j ar OUTSCI 
pula 

ataa tdata 

jar OUTSCI 

PRTHETX puly 
pulx 
pulb 
pula 
rta 


* P RHODE — Print mode and modeflag 


PRMODE paha 
pshb 
pahx 
pahy 

ldaa mode 

ataa tdata 

jsr OUTSCI 

ldaa modeflag 

staa tdata 

j sr OUTSCI 

puly 

pulx 

pulb 

pula 

rts 


* PREST -- Print estimated theta 


PREST paha 
pshb 
pshx 
pshy 

ldaa theta_e 
jar TOASCII 
pshb 

staa tdata 
jsr OUTSCI 
pula 

staa tdata 
jsr OUTSCI 

ldaa theta_e+l 
jsr TOASCII 
pshb 

staa tdata 
j sr OUTSCI 
pula 

staa tdata 
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jar OUTSCI 


puly 

pulx 

pulb 

pula 

rts 


8-bit binary in A -> 2 ascii digits in A:B 


TO ASCII tab 
rora 
rora 
rora 
rora 

anda #$0F 
adda #$30 
caqpa #$39 
bla TASC1 
adda #7 

TASC1 andb #$0F 
addb #$30 
cmpb #$39 
bla TASCX 
addb #7 
TASCX rts 


* WRITE A SPACE TO THE SCI 
WRSPACE psha 
pohb 

ldaa #$20 

staa tdata 

jsr outsci 

pulb 

pula 

rts 


* STEPIT -- Calculate and sand motor c omm and. 


STEPIT NOP 

* Sava old accumulator values 

psha 

pshb 

pshx 

pshy 

* Reset steps 

Idd #$0000 

std steps 

* Calculate new estimate for theta using theta_e and steps (number of 

* steps since last update. 


STEPIT1 nop 
ldd 

steps 

cpd 

#$0000 

bit 

netback 

bra 

netfwd 

netback ldd 

#$0000 

subd 

steps 

Ur 

stp2deg 

subd 

theta_e 

std 

theta__e 

ldd 

#$0000 

subd 

theta_e 

std 

theta_e 
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bra 


newest 


netfwd Idd 

steps 


j«r 

stp2deg 


addd 

theta_e 


std 

theta_e 


bra 

newest 


newest ldd 

#$0000 


std 

steps 


* Determine mode of operation 


jar 

RDCNTR 

* 

j«r 

DIV2DKG 

* 

Idaa 

mode 

* 

cmpa 

#'o* 


beq 

CONTI 


ldd 

theta 

★ 

subd 

theta_e 


std 

cnterr 


cpd 

#$0000 

* 

bge 

MODECHK 


ldd 

#$0000 


subd 

cnterr 


MODECHK cpd 

MAXERR 

* 

bgt 

OPEN3 


bra 

CLOSED3 

* 

OPEN3 ldaa 

# 1 o ' 


staa 

mode 


ldaa 

# ' 3 ’ 


staa 

modef lag 


bra 

CONTI 


CLOSED3 ldaa 

# ' c * 


staa 

mode 


ldaa 

#'0* 


staa 

modef lag 


bra 

CONTI 


* Determine number and direction 

CONTI ldaa 

fstop 

* 

cmpa 

#* 1* 

* 

bne 

CONT2 

* 

ldaa 

#’0' 

* 

staa 

fstop 

* 

ldaa 

rdata 

* 

cmpa 

#’S’ 

* 

bne 

CO NT 2 

* 

ldaa 

mode 


cmpa 

# 1 o 1 


beq 

olstop 


ldd 

theta 

* 

bra 

storit 


olstop ldd 

theta_e 


storit std 

theta_d 

* 

* Calculate 
CONT2 nop 

delta theta 


ldaa 

mode 


cmpa 

# * c 1 


beq 

CLOSED 

* 

ldd 

theta_d 

★ 

subd 

theta_e 

* 

std 

delta_th 

* 

bra 

MOVEON 

* 


* if open loop mode goto CONTI 


* cnterr ■ theta - theta_e 


* cnterr * abs (cnterr) 


* if cnterr>MAXERR goto 0PBN3 


* jump to CLOSED3 


f stop - 'O' 
if rdata = 'S’ 

if modesclosed 

theta_d ■ theta 

else 

theta_d =* theta_ 

end if 


if mode ® closed then 

delta_th ■ theta_d - theta 

else 

delta_th ■ theta_d - theta. 
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CLOSED 

Idd 

theta_d 




subd 

theta 




8td 

delta_th 



* Use delta 

theta to determine motor direction 

MOVEON 

pah b 
psha 
puly 


* 

Y * delta_th 


cpy 

#$FFFF 




bit 

JGOREV 


* if delta_th < -1 GOREV 


cpy 

#$0001 




bgt 

GOFWD 

★ 

if delta_th > 1 GOFWD 


jmp 

STEPITX 

★ 

branch to STEPITX (Good enough! ) 

JGOREV 

jmp 

GOREV 



* Move 

motor 

forward if 

necessary 

GOFWD 

ldd 

theta 




std 

theta_o 




cpy 

#$00B4 




ble 

GOFWD1 




Idy 

#$00B4 



GOFWD1 

nop 

pahy 

pula 

pulb 


* 

B - LSB Of Y (MSB * $00) 


Idaa 

#$0A 

* 

A - $0A (10) 


mul 


* 

D ■ B*$0A 


ldx 

#$09 

* 

X * $09 (9) 


idiv 

pahx 


* 

X * LSB*10/9 


puly 


* 

Y « X (steps) 

GOFWD2 

ldaa 

#*1‘ 




cznpa 

f atop 




bne 

fwda 




jmp 

STEPIT1 



fwda 

ldd 

atepa 




addd 

#$01 




atd 

atepa 




jar 

MFWD1 

* 

Take one step forward 


jar 

SLODOWN 

* 

Pause between steps 


dey 


* 

Y - Y - 1 


cpy 

#$0000 




bne 

GOFWD2 

* 

If Y <> 0 jump to GOFWD 2 


ldaa 

mode 

* 

if open loop mode goto C0NT3 


cmpa 

# 1 o 1 




beq 

CONT3 




ldd 

theta_o 




jar 

RDCNTR 

* 

read counter value into byte2,bytel,byte0 


jar 

DIV2DEG 

* 

convert counter value to degrees (theta) 


cpd 

theta 




beq 

F0PEN1 




jmp 

STEPIT1 

* 

jump to STSPITl 

FOPEN1 

ldaa 

# 1 o 1 




ataa 

mode 




ldaa 

#'1 ( 




ataa modeflag 



CONT3 

jmp 

STEPIT1 



* Hove 

motor 

backward if 

necessary 

GOREV 

ldd 

theta 




atd 

theta_o 




ldd #$0000 

* D = 0 


subd 

pshb 

delta_th 

* 

D * D - delta_th 


paha 





puly 

cpy 

#$00B4 

* 

Y = D (Y « -delta_th) 
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ble GOREV1 
ldy #$00B4 
GOREV1 nop 
pshy 
pula 
pulb 

ldaa #$0A 
mul 

Idx #$09 
idiv 
pshx 
puly 
GOREV2 nop 

ldaa # * 1 * 

cmpa fstop 

bne rava 

jmp STEPIT1 

rava ldd steps 

subd #$01 

std steps 

jsr MREV1 
jsr SLODOWN 
day 

cpy #$0000 
bna GOREV2 


* B = LSB of Y (MSB - $00) 

* A * $0A (10) 

* D * B*$0A 

* X » $09 (9) 

* X * LSB* 10/9 

* Y ■ X (stops) 


* Taka one step backward 

* Pause between steps 

* Y ■ Y - 1 

* If Y <> 0 jump to GOREV2 


ROPEN1 


C0NT4 


ldaa 

mode 

cmpa 

# ' o * 

beq 

CONT4 

ldd 

theta_o 


RDCNTR 

jar 

DXV2DEC 

cpd 

theta 

beq 

ROPEN1 

jmp 

STEPIT1 

ldaa 

# * o 1 

staa 

mode 

ldaa 

#•!' 

staa modeflag 

jmp 

STEPIT1 


* if open loop mode goto CO NT 4 


* read counter value into byte2,bytel,byte0 

* convert counter value to degrees (theta) 


* Jump to STEPIT1 


* Jump to STEPIT1 


* * Recall old accumulator values 

STEPITX puly 
pulx 
pulb 
pula 
rts 


* Issue command for one step backward 
MREV1 psha 

pshb 

pshx 

pshy 

ldx #RZ6BAS 

ldab PORTA, X 

andb #*11100111 

stab PORTA, X 

orab #*00001000 

stab PORTA, X 

puly 

pulx 

pulb 

pula 

rts 

* Issue command for one step forward 
MFWD1 psha 

pshb 
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pshx 

P*hy 

ldx #REGBA 3 
Ida b PORTA, X 
orab #*00010000 
andb #*11110111 
stab PORTA, X 
orab **00001000 
stab PORTA, X 


puly 

pulx 

pulb 

pula 

rts 


* HEXBIN(a) - Convert the ASCII character in a 

* to binary and shift into shftreg. Returns value 

* in TMP1 incremented if a is not hex. 


HEXBIN PSHA 
PSHB 
PSHX 
pshy 

JSR UPCASE 
CMPA # , 0‘ 

BLT HEXNOT 
CMPA #'9' 

BLE HEXNMB 
CMPA # ' A* 

BLT HEXNOT 
CMPA # ' F * 

BGT HEXNOT 
ADDA #$9 

HEXNMB ANOA *$0F 

LDX #SHFTREG 
LDAB *4 

HEXSHFT ASL 1,X 
ROL 0,X 
DECB 

BGT HEXSHFT 
ORAA 1,X 
STAA 1,X 
BRA HEXRTS 
HEXNOT INC TMP1 
HEXRTS puly 
PULX 
PULB 
PULA 
RTS 


* UPCASE(a) - If the contents of A is alpha, 

* returns a converted to uppercase. 


UPCASE CMPA * • a ' 

BLT UPCASE 1 jump if < a 

CMPA #'z' 

BGT UPCASE 1 jump if > z 

SUBA #$20 convert 

UPCASE 1 RTS 


★ DXV2DEG — Converts Count (in divisions) to theta (in degrees) 


DIV2DEG psha 
pshb 
pshx 

pshy * Push Y onto stack 


convert to upper case 

jump if a < $30 

jump if 0-9 

jump if $39> a <$41 

jump if a > $46 
convert $A-$F 
convert to binary 

2 byte shift through 
carry bit 

shift 4 times 
indicate not hex 
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ld&a 
pa ha 

TMP1 


ldaa 

*$00 


staa 

TMP1 

* TMP1 ■ 0 

ldy 

*$0000 

* Y - 0 

Idd 

bytal 

* D * COUNT (in divisions) (512 div/rev) 

cpd 

*$0000 


bga 

NOCHNG 

* If byte >« 0, jump to NDCHNG 

ldd 

*$0000 


subd 

psha 

bytal 

* D - -bytal :byta0 

ldaa 

#1 


staa 

pula 
NOCHNG nop 

TMP1 

* TMP1 - 1 

* Determine how many times D is 

divisible by $100, $100 div ■ $B4 deg. 

CNT1 cpd 

*$0100 


bla 

CNT2 

★ Jump to CNT2 if D <» $0100 

subd 

*$0100 

* D « D - $100 

iny 


* Y * Y + 1 

bra 

CNT1 

* Jump always to CNT1 

CNT2 ldaa 

#$2D 

* A - $2D (45) 

mul 


* D * A * B (B is LSB of COUNT, MSB » $00) 

ldx 

*$0040 

* IX - $40 (€4) 

idiv 


* IX » D/IX 

pshx 


* Push X onto Stack 

pula 


* Pull A off stack (A » MSB of D) 

pulb 


* Pull B off stack (B * LSB of D) 

std 

thata 

* thata ■ IX (second term of thata) 

pshy 


* Push Y onto stack 

pula 


* Pull A off stack (A * MSB of IY) 

pulb 


* Pull B off stack (B * LSB of IY) 

ldaa 

#$B4 

* A a $B4 (180) 

mul 


* D ■ A * B 

addd 

thata 

* D * D + thata 

std 

thata 

* thata * D 

ldaa 

TMP1 

* Look at TMPl 

cmpa 

*1 

* 

bna 

NOCHNG2 

* If TMP1 is not equal to 1, jump to NOCHNG2 

ldd 

*$0000 

* 

subd 

thata 

* 

std 

NOCHNG2 nop 
pula 

thata 

* thata » -thata 

staa 

TMP1 


puly 


* Pull Y off stack 

pulx 


* Pull X off stack 

pulb 


* Pull B off stack 

pula 

rts 


* Pull A off stack 

* stp2deg -- 

Converts steps in 

D (4 00 /rev) to degrees (360/rev) in D. 

atp2deg pshx 


* Push X onto stack 

pshy 


* Push Y onto stack 

std 

tmpl6 


s2dl ldd 

*$0000 


ldy 

#9 


mul It addd 

day 

tnplS 


bna 

mul it 


std 

tmpl 6 
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ldx 

idiv 

#10 

stx 

pshx 

trap 16 

pula 


pulb 

std 

trap 16 

puly 

pulx 

rts 



* Pull Y off stack 
* Pull X off stack 


* SCI Int erupt Service Routine 

* If first byte is fa or ADDRESS, 

* dxflag is set to $31 and 

* remaining bytes are stored in rdata 


SCI_ISR 

*** 

ldx 

#REGBAS 


ldaa 

SCSR, X 


ldaa 

SCDR, X 


crape 

#$fa 


beq 

isr2 


crap a 

ADDRESS 


beq 

isr2 


jmp 

isr4 

isr2 

ldy 

frdata 

isr3 

brclr 

SCSR,X #*00100000 


ldaa 

SCDR, X 


staa 

0 , Y 


iny 

cpy 

#COMSIZE+#R_DATA 


bio 

isr3 

* if rdata : rdata+3 * 'S***' then 

* 

fstop 

- '1* 

* 

rti 


* end 

ldaa 

rdata 


crap a 

#'S’ 


bne 

nostop 


ldaa 

#‘l' 


staa 

fstop 


jmp 

isr4 

nostop 

nop 



* necessary to reset flag 

* read first byte 

* if bytel .eq. $fa then goto isr2 

* if bytel .ne. ADDRESS goto isr4 


* stop here until byte is received 

* read fie save byte 

* increment pointer 

* compare pointer to max pointer 

* if all bytes not read goto isr3 


* A - rdata 


* fstop = ' 1‘ (valid data) 

* rti 


* if all rdata byte 0-9 or A-P then 

* set flag 

* else 

* rdata *» 0000 

* end 

* return 


ldaa 

#$00 

staa 

TMP1 

ldy 

# rdata 

ldaa 

0 , Y 

jar 

HEXBIN 

ldaa 

TMP1 

bne 

isr4 

ldaa 

1# Y 

jar 

HEXBIN 

ldaa 

SHFTREG+1 

staa 

theta_d 

ldaa 

2 , Y 

jar 

HEXBIN 

ldaa 

TMP1 

bne 

isr4 
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ldaa 3,Y 
j ar HEXBIN 
ldaa SHFTREG+1 
staa theta_d+l 
ldaa #$31 

staa dxflag 

ldd theta_d 

cpd lim_min 

bge LXM1 

ldd lim_min 

std theta_d 

LIMl cpd lim_max 

ble LIM2 

ldd lim_max 

atd theta_d 

LIM2 nop 

lsr4 nop 

bset SCCR2 , X #*00000010 

za_iar rti 


* set good data received flag 

* if thata_d < lim_min than 

* thata_d * lim_min 

* andif 

* if thata_d > lim_max than 

* theta__d » lim_max 

* andif 


* RWU ■* 1 (MCU in wakeup xnoda) 
* return from intarupt 


* STARTUP -- MAIN 


STARTUP Ida #STACICTOP 
jar INIT 

ldaa #CR * Doaa not affect master. Looks batter on screen, 

staa tdata 
jar OUTSCI 

LOOPO jar RDCNTR 
ldaa #$f0 
staa tdata 
j sr OUTSCI 
ldaa #$f0 
staa tdata 
j sr OUTSCI 

ldy #$FF * pause to allow master to select port 

pause 1 day 

bna pausal 

ldaa mode 
cxnpa # ’ c ' 
beq CLLOOP 
j sr PRIEST 
bra GO LOOP 
CLLOOP j sr PRTHETA 
GO LOOP j sr PRMODE 

* Write command to SCI. 

* Check for received command 
LOOPl ldaa dxflag 

cmpa #$31 
bna LOOPl 
jar STEPIT 
ldaa #$00 
staa dxflag 
ZLOOPO bra LOOPO 


* master address 

* master address 
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E. Using Pcbugll to Program the Motorola 68HC11E9 
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PAGE f$ . 

CM? oi‘*' ,TV S68HC1 iEVBtl/Li 

S68HC1 1EVBU Customer 

Both the standard and stddent EVBUs come with 3ft MC68BC11E9FN1 MCtJ installed ofi the 
board and the BUFFALO monitor program Stated in the MCU- internal ROM. But the Student 
EVBU kit also contains a blank MC68HC711E9 MCU and PCbugll, a PC-based monitor 
program. A complete description of the PCbugll is provided in the PCbugll User'S Manual, 
M68PCBUG11/D1. While a detailed description of the BUFFALO monitor is a vailab le in the 
M68HC1 1EVBU Universal Evaluation Board User's Manual, M68HC1 1EVBU/AD1. 

You may install the blank MC68HC711E9 MCtJ ih the EVBU socket at location tJ3. After 
installing the 711E9 MCU on the EVBU it may be programmed using PCbUgii. Either 
BUFFALO or a user-developed program can be stortdin the 71 1E9 MCU-intSflal EPROM Step- 
by-step EPROM programming instructions a it provided in this letter or refer to page 4-12 of the 
PCbugll user's manual for additional information. 


WHAT IS PCbugll? 

PCbugll is a software package for easy access to and simple experimentation with M68HC11 
microcontroller unit (MCU) devices. PCbugll lets you program any member Of the M68HC11 
MCU family and examine the behavior of internal peripherals under Specific conditions. In 
addition, you may run your own programs on the MCU; breakpoint processing and trice 
processing are available. 

To configure the EVBU to use PCbugl 1 : 

i . Remove the jumper from J7, and place it across 13. Moving the jumper to J3 grounds 
the MODB pin and at reset places the HCi 1 in BOOTSTRAP mode. 


NOTE 

Refer to Figure 2-1 of the M68MC1 1EVBU Universal Evaluation 
Board User's Manual, M68HC11EVBU/AD1, for jumper header 
and connector locations. 


2. Connect the EVBU to your PC serial port Via a user-supplied 25-pin cable. The PC 
serial port can be either COM1 or COM2. The cable must be a Hayes-compatible 
modem cable and is available at most electronic supply stores. 


3. Apply power to the EVBU. 


UNIVERSAL EVALUATION BOARD CUSTOMER LETTER 
S68HC11EVBU/L1 4+f 

II Z 
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4. 1*6 start PCbugl 1 from the Command line: 


PAOt ts 

*W .PC^P fv » ft 


f Tv 


BCbtlgll -E pOrt=l<CR> Whe- t/O is COM2, use port=2. 

<CIo- is the symbol for carriage return. 


5 . The registers should be displayed oil the Screen, and a » prompt in the window at the 
bottom of the screen. 


6. With PCbugl 1 version 3.24A, enter oft the PC keyboard: 

CONTROL BASE HEX<CR> 

This defines the keyboard input default as hexadecimal. By doing this, you do not have 
to add the $ to inputs. 


This should get you started with PCbugl 1. Because the TALKER code used iii this example 
resides in RAM, you are limited to the amount of free space that you can use for variables. It may 
be useful to put the TALKER into EPROM (it takes about 200 bytes), and leave ffiOSt of yohf tlSfet 
space free. For more detail oft the TALKER refer to paragraph 4.4 of the PCbUg 1 1 manual. 
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programming eeprom '■* p0<m 0 !A 

Files to b8 programmed into the 71 1E9 MCU-iritefnal EEPROM must be in S -record format The 
S-recdtd for mat is explained in Appendix A of the M68HC1 1EVBU Universal Evaluation Board 
User's Manual, M68HC1 1EVBU/AD1. 

NOtE 

The S -record to be downloaded iritb the 711E9 MCU-intertial 
EEPROM must be ORGed at addrtSS $B600. 


Enter on the PC keyboard: 

EEPROM $8600 $R7EE<CR> This lets PCbugll know that these addresses ate 

EEPROM and that it should use a EEPROM 
algorithm. 

MS $1035 00<CR> This dears the block protect register (8PR6T) and 

lets you program the EEPftOM section of the 71 1E9 
MCU. 

LOADS filename<CR> This loads an S-record format file into the EEPROM 

section Of the 71 1E9 Mcu. 

VERP fiIename<CR> This verifies that the S-recofd format file was 

successfully loaded into EEPROM. 
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flfiari - f .v * 

OK HOC* r/jftt n-Y 

PROGRAMMING EPROM 

Files to bft p ro gr a mmed into the 71 1E9 MCU-intenial EPROM must be in S-tecotd format The S- 
record format is explainer : n Appendix A of the M68HC1 1EVBU Universal Evaluation Board 
User's Manual, M68HC1 1EYBU/AD1. 

NOfE 

The S -record to be downloaded into the 711E9 MCU-intefnal 
EPROM must be ORGed at address $D000. 


To program the MCU-intemal EPROM enter on the PC keyboard: 

EPROM $D00d $FEFF<cR> this lets Pcbugii know that these addtesses are 

EPROM and that it should use a EPROM algorithm. 

Apply +12Vdc tn the XirQ pm Td program the MCU-intemal EPROM* +12Vde 

must be applied to the xirq pin a f the 71119 McU; 
Attach a +i2Vdc power supply ta the Mcu Vo port 
edaneetor P4, pin- is. A lodQ resister must be 
installed in series with the 4-llVde power supply and 
P4, pin- 18. 

CAUTION 

Do not apply a +12Vdc programming Voltage power source when 
the main Vdd (+5Vdc) power is Off; doing so will damage the 
EVBU intrgrated circuits. Always turn OH the main Vdd (+5Vdc) 
power before the +12Vdc p ro gr amming Voltage is applied. 

LOADS fiienntne<CR> This loads an s-record format file into the EPROM 

section df the 71 1E9 MCU. 

VERP filehanie<CR> This verifies that the S-record format file was 

successfully loaded into EPROM. 
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PCbugll HINTS 


OK rye - 

* The EPROM and EEPROM commands mUSt be entered befoi* you cad program 
EPROM and EEPROM. This sets-up PCbugll EPROM and EEPROM programming 
routines. 

* Don’t forget to clear the BPROT register before trying to modify EEPROM locations. 

* Initially, you Should work on your routines in EEPROM. Since you can trade through 
EEPROM like RAM, it is best to try them out there before committing to EPROM. When 
tracing the EEPROM Use the memory set (MS) command to modify the block pietect 
register to 00 (MS $1035 00) and the EEPROM command (EEPROM $B600 $B7fP). 

« Re sum to set your stack pointer where it will not interfere with the PCbugll stack 
pointer. The TALKER program starts at $0000 in RAM, with the first free byte at 
$0100. The PCbUgl 1 stack pointer is Set to $01FF. Set your stack pointer at least 20 
bytes ($01EB) lower than this. 

* if a COM fault occurs while entering commands on the command line: 

i . Check the cable between the EVBU and the pc. If the connection is okay, try 
issuing the control timeout command (CONTROL TIMEOUT 10000). This gives 
the MCU mom time to respond (needed when PCbugi l is running on a fast PC). 

1 . Make sure the transmit pin for the PC connects to the receive pin of the MCU. The 
transmit pin will have approximately 9 to 12 volts on it The receive pin will only 
have a few millivolts if any. 

3. Remove the MCU from the socket and check the pins for damage, if the pins are 
shorted, straighten the pins and carefully reinsert the MCU. 

* If you am using an XT-PC and the display locks-up, try issuing a MODE CO80 at the 
DOS prompt 
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